Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[build] Apply spotless for java formatting (#1768)

Browse Source 
Update checkstyle config to be compatible with spotless.

Co-authored-by: Austin Shalit <austinshalit@gmail.com>
This commit is contained in:
Peter Johnson
2020-12-29 22:45:16 -08:00
committed by GitHub
parent e563a0b7db
commit a751fa22d2
883 changed files with 16526 additions and 17751 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
View File

@@ -8,42 +8,35 @@ 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.
* 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.
*/
public interface Command {
/**
* The initial subroutine of a command. Called once when the command is initially scheduled.
*/
default void initialize() {
}
/** 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 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.
* The action to take when the command ends. Called when either the command finishes normally, or
* when it interrupted/canceled.
*
* <p>Do not schedule commands here that share requirements with this command.
* Use {@link #andThen(Command...)} instead.
* <p>Do not schedule commands here that share requirements with this command. Use {@link
* #andThen(Command...)} instead.
*
* @param interrupted whether the command was interrupted/canceled
*/
default void end(boolean interrupted) {
}
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.
* 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.
*/
@@ -52,29 +45,28 @@ public interface Command {
}
/**
* 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.
* 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.
* <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.
* 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
* <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.
* 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
@@ -84,16 +76,16 @@ public interface Command {
}
/**
* 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.
* 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
* <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.
* 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
@@ -105,13 +97,13 @@ public interface Command {
/**
* 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
* <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.
* 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
* @param toRun the Runnable to run
* @param requirements the required subsystems
* @return the decorated command
*/
@@ -122,13 +114,13 @@ public interface Command {
/**
* 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
* <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.
* 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
* @param toRun the Runnable to run
* @param requirements the required subsystems
* @return the decorated command
*/
@@ -137,14 +129,14 @@ public interface Command {
}
/**
* Decorates this command with a set of commands to run after it in sequence. Often more
* 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
* <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.
* 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
@@ -157,14 +149,14 @@ public interface Command {
/**
* 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
* 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
* <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.
* 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
@@ -175,14 +167,14 @@ public interface Command {
/**
* 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.
* 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
* <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.
* 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
@@ -195,14 +187,14 @@ public interface Command {
/**
* 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.
* 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
* <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.
* 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
@@ -214,14 +206,14 @@ public interface Command {
}
/**
* Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated
* 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
* <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.
* 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
*/
@@ -230,9 +222,9 @@ public interface Command {
}
/**
* 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.
* 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
*/
@@ -243,32 +235,29 @@ public interface Command {
/**
* Schedules this command.
*
* @param interruptible whether this command can be interrupted by another command that
* shares one of its requirements
* @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.
*/
/** 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.
* 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.
* 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.
*/
@@ -277,10 +266,10 @@ public interface Command {
}
/**
* 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.
* 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
@@ -290,8 +279,8 @@ public interface Command {
}
/**
* Whether the given command should run when the robot is disabled. Override to return true
* if the command should run when disabled.
* 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
*/

51
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
View File

@@ -4,16 +4,13 @@
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;
import java.util.HashSet;
import java.util.Set;
/**
* A {@link Sendable} base class for {@link Command}s.
*/
/** A {@link Sendable} base class for {@link Command}s. */
@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
public abstract class CommandBase implements Sendable, Command {
@@ -54,12 +51,11 @@ public abstract class CommandBase implements Sendable, Command {
}
/**
* Decorates this Command with a name.
* Is an inline function for #setName(String);
*
* @param name name
* @return the decorated Command
*/
* Decorates this Command with a name. Is an inline function for #setName(String);
*
* @param name name
* @return the decorated Command
*/
public Command withName(String name) {
this.setName(name);
return this;
@@ -86,7 +82,7 @@ public abstract class CommandBase implements Sendable, Command {
}
/**
* Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
* Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
*
* @param builder the builder used to construct this sendable
*/
@@ -94,18 +90,21 @@ public abstract class CommandBase implements Sendable, Command {
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);
builder.addBooleanProperty(
"running",
this::isScheduled,
value -> {
if (value) {
if (!isScheduled()) {
schedule();
}
} else {
if (isScheduled()) {
cancel();
}
}
});
builder.addBooleanProperty(
".isParented", () -> CommandGroupBase.getGroupedCommands().contains(this), null);
}
}

14
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
View File

@@ -10,9 +10,9 @@ 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.
* 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 =
@@ -25,8 +25,8 @@ public abstract class CommandGroupBase extends CommandBase implements Command {
/**
* 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.
* <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();
@@ -36,8 +36,8 @@ public abstract class CommandGroupBase extends CommandBase implements Command {
* 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.
* <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
*/

216
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View File

@@ -4,17 +4,6 @@
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;
@@ -27,19 +16,27 @@ import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
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;
/**
* The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link
* 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.
* 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.TooManyFields"})
public final class CommandScheduler implements Sendable, AutoCloseable {
/**
* The Singleton Instance.
*/
/** The Singleton Instance. */
private static CommandScheduler instance;
/**
@@ -54,24 +51,24 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
return instance;
}
//A map from commands to their scheduling state. Also used as a set of the currently-running
//commands.
// 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.
// 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.
// 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.
// The set of currently-registered buttons that will be polled every iteration.
private final Collection<Runnable> m_buttons = new LinkedHashSet<>();
private boolean m_disabled;
//Lists of user-supplied actions to be executed on scheduling events for every command.
// 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<>();
@@ -83,22 +80,24 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
private final List<Command> m_toCancel = new ArrayList<>();
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> { });
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
CommandScheduler() {
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
SendableRegistry.addLW(this, "Scheduler");
LiveWindow.setEnabledListener(() -> {
disable();
cancelAll();
});
LiveWindow.setDisabledListener(() -> {
enable();
});
LiveWindow.setEnabledListener(
() -> {
disable();
cancelAll();
});
LiveWindow.setDisabledListener(
() -> {
enable();
});
}
/**
* Changes the period of the loop overrun watchdog. This should be be kept in sync with the
* Changes the period of the loop overrun watchdog. This should be be kept in sync with the
* TimedRobot period.
*
* @param period Period in seconds.
@@ -123,9 +122,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
m_buttons.add(button);
}
/**
* Removes all button bindings from the scheduler.
*/
/** Removes all button bindings from the scheduler. */
public void clearButtons() {
m_buttons.clear();
}
@@ -133,9 +130,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
/**
* Initializes a given command, adds its requirements to the list, and performs the init actions.
*
* @param command The command to initialize
* @param command The command to initialize
* @param interruptible Whether the command is interruptible
* @param requirements The command requirements
* @param requirements The command requirements
*/
private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
CommandState scheduledCommand = new CommandState(interruptible);
@@ -152,13 +149,13 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Schedules a command for execution. Does nothing if the command is already scheduled. If a
* 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
* 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
* @param command the command to schedule
*/
@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
private void schedule(boolean interruptible, Command command) {
@@ -172,21 +169,22 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
"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())
// 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.
// 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.
// 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()) {
@@ -203,13 +201,13 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* 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,
* 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
* @param commands the commands to schedule
*/
public void schedule(boolean interruptible, Command... commands) {
for (Command command : commands) {
@@ -218,7 +216,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
* Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
* if the command is already scheduled.
*
* @param commands the commands to schedule
@@ -228,7 +226,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Runs a single iteration of the scheduler. The execution occurs in the following order:
* Runs a single iteration of the scheduler. The execution occurs in the following order:
*
* <p>Subsystem periodic methods are called.
*
@@ -248,7 +246,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
m_watchdog.reset();
//Run the periodic method of all registered subsystems.
// Run the periodic method of all registered subsystems.
for (Subsystem subsystem : m_subsystems.keySet()) {
subsystem.periodic();
if (RobotBase.isSimulation()) {
@@ -257,16 +255,16 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
}
//Poll buttons for new commands to add.
// Poll buttons for new commands to add.
for (Runnable button : m_buttons) {
button.run();
}
m_watchdog.addEpoch("buttons.run()");
m_inRunLoop = true;
//Run scheduled commands, remove finished commands.
// Run scheduled commands, remove finished commands.
for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
iterator.hasNext(); ) {
iterator.hasNext(); ) {
Command command = iterator.next();
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
@@ -298,7 +296,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
m_inRunLoop = false;
//Schedule/cancel commands from queues populated during loop
// Schedule/cancel commands from queues populated during loop
for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
}
@@ -310,7 +308,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
m_toSchedule.clear();
m_toCancel.clear();
//Add default commands for un-required registered subsystems.
// 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) {
@@ -326,10 +324,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* 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.
* 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
*/
@@ -340,7 +337,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic
* 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
@@ -350,13 +347,13 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* 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.
* 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 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) {
@@ -372,7 +369,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Gets the default command associated with this subsystem. Null if this subsystem has no default
* 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
@@ -383,9 +380,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Cancels commands. The scheduler will only call {@link Command#end(boolean)} method
* of the canceled command with {@code true},
* indicating they were canceled (as opposed to finishing normally).
* Cancels commands. The scheduler will only call {@link Command#end(boolean)} method of the
* canceled command with {@code true}, indicating they were canceled (as opposed to finishing
* normally).
*
* <p>Commands will be canceled even if they are not scheduled as interruptible.
*
@@ -412,9 +409,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
}
/**
* Cancels all commands that are currently scheduled.
*/
/** Cancels all commands that are currently scheduled. */
public void cancelAll() {
for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
cancel(command);
@@ -422,7 +417,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Returns the time since a given command was scheduled. Note that this only works on commands
* 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.
*
@@ -439,9 +434,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* 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.
* 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
@@ -451,7 +446,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
/**
* Returns the command currently requiring a given subsystem. Null if no command is currently
* 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
@@ -461,16 +456,12 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
return m_requirements.get(subsystem);
}
/**
* Disables the command scheduler.
*/
/** Disables the command scheduler. */
public void disable() {
m_disabled = true;
}
/**
* Enables the command scheduler.
*/
/** Enables the command scheduler. */
public void enable() {
m_disabled = false;
}
@@ -517,34 +508,33 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
final NetworkTableEntry namesEntry = builder.getEntry("Names");
final NetworkTableEntry idsEntry = builder.getEntry("Ids");
final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
builder.setUpdateTable(() -> {
builder.setUpdateTable(
() -> {
if (namesEntry == null || idsEntry == null || cancelEntry == null) {
return;
}
if (namesEntry == null || idsEntry == null || cancelEntry == null) {
return;
}
Map<Double, Command> ids = new LinkedHashMap<>();
Map<Double, Command> ids = new LinkedHashMap<>();
for (Command command : m_scheduledCommands.keySet()) {
ids.put((double) command.hashCode(), command);
}
double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
if (toCancel.length > 0) {
for (double hash : toCancel) {
cancel(ids.get(hash));
ids.remove(hash);
}
cancelEntry.setDoubleArray(new double[0]);
}
for (Command command : m_scheduledCommands.keySet()) {
ids.put((double) command.hashCode(), command);
}
List<String> names = new ArrayList<>();
double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
if (toCancel.length > 0) {
for (double hash : toCancel) {
cancel(ids.get(hash));
ids.remove(hash);
}
cancelEntry.setDoubleArray(new double[0]);
}
ids.values().forEach(command -> names.add(command.getName()));
List<String> names = new ArrayList<>();
ids.values().forEach(command -> names.add(command.getName()));
namesEntry.setStringArray(names.toArray(new String[0]));
idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
});
namesEntry.setStringArray(names.toArray(new String[0]));
idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
});
}
}

7
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
View File

@@ -7,14 +7,13 @@ 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 that holds scheduling state for a command. Used internally by the {@link CommandScheduler}.
*/
class CommandState {
//The time since this command was initialized.
// The time since this command was initialized.
private double m_startTime = -1;
//Whether or not it is interruptible.
// Whether or not it is interruptible.
private final boolean m_interruptible;
CommandState(boolean interruptible) {

16
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
View File

@@ -4,17 +4,17 @@
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;
import java.util.function.BooleanSupplier;
/**
* 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}.
* 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
@@ -31,8 +31,8 @@ public class ConditionalCommand extends CommandBase {
/**
* 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 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) {

22
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
View File

@@ -4,14 +4,14 @@
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
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
* 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.
*/
@@ -24,14 +24,18 @@ public class FunctionalCommand extends CommandBase {
/**
* 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 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) {
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");

13
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
View File

@@ -7,9 +7,9 @@ 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.
* 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;
@@ -17,7 +17,7 @@ public class InstantCommand extends CommandBase {
/**
* Creates a new InstantCommand that runs the given Runnable with the given requirements.
*
* @param toRun the Runnable to run
* @param toRun the Runnable to run
* @param requirements the subsystems required by this command
*/
public InstantCommand(Runnable toRun, Subsystem... requirements) {
@@ -27,12 +27,11 @@ public class InstantCommand extends CommandBase {
}
/**
* Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg
* 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 = () -> {
};
m_toRun = () -> {};
}
@Override

496
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -4,8 +4,7 @@
package edu.wpi.first.wpilibj2.command;
import java.util.function.Consumer;
import java.util.function.Supplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
@@ -19,28 +18,24 @@ import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import java.util.function.Consumer;
import java.util.function.Supplier;
/**
* A command that uses two PID controllers ({@link PIDController}) and a
* ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
* {@link Trajectory} with a mecanum drive.
* A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
* ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a mecanum drive.
*
* <p>The command handles trajectory-following,
* Velocity 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>The command handles trajectory-following, Velocity 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 PID
* controllers.
* <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 PID controllers.
*
* <p>The robot angle controller does not follow the angle given by
* the trajectory but rather goes to the angle given in the final state of the trajectory.
* <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
* to the angle given in the final state of the trajectory.
*/
@SuppressWarnings({"PMD.TooManyFields", "MemberName"})
public class MecanumControllerCommand extends CommandBase {
private final Timer m_timer = new Timer();
@@ -70,85 +65,79 @@ public class MecanumControllerCommand extends CommandBase {
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param feedforward The feedforward to use for the drivetrain.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller
* for the robot's x position.
* @param yController The Trajectory Tracker PID controller
* for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller
* for angle for the robot.
* @param desiredRotation The angle that the robot should be facing. This
* is sampled at each time step.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param feedforward The feedforward to use for the drivetrain.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param desiredRotation The angle that the robot should be facing. This is sampled at each time
* step.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param frontLeftController The front left wheel velocity PID.
* @param rearLeftController The rear left wheel velocity PID.
* @param frontRightController The front right wheel velocity PID.
* @param rearRightController The rear right wheel velocity PID.
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
* the current wheel speeds.
* @param outputDriveVoltages A MecanumDriveMotorVoltages object containing
* the output motor voltages.
* @param requirements The subsystems to require.
* @param frontLeftController The front left wheel velocity PID.
* @param rearLeftController The rear left wheel velocity PID.
* @param frontRightController The front right wheel velocity PID.
* @param rearRightController The rear right wheel velocity PID.
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
* @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
* voltages.
* @param requirements The subsystems to require.
*/
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
public MecanumControllerCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
SimpleMotorFeedforward feedforward,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
PIDController rearRightController,
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory",
"MecanumControllerCommand");
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SimpleMotorFeedforward feedforward,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
PIDController rearRightController,
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
m_feedforward = requireNonNullParam(feedforward, "feedforward",
"MecanumControllerCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics",
"MecanumControllerCommand");
m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
m_controller = new HolonomicDriveController(
requireNonNullParam(xController,
"xController", "SwerveControllerCommand"),
requireNonNullParam(yController,
"xController", "SwerveControllerCommand"),
requireNonNullParam(thetaController,
"thetaController", "SwerveControllerCommand")
);
m_controller =
new HolonomicDriveController(
requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
"MecanumControllerCommand");
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
"maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
m_maxWheelVelocityMetersPerSecond =
requireNonNullParam(
maxWheelVelocityMetersPerSecond,
"maxWheelVelocityMetersPerSecond",
"MecanumControllerCommand");
m_frontLeftController = requireNonNullParam(frontLeftController,
"frontLeftController", "MecanumControllerCommand");
m_rearLeftController = requireNonNullParam(rearLeftController,
"rearLeftController", "MecanumControllerCommand");
m_frontRightController = requireNonNullParam(frontRightController,
"frontRightController", "MecanumControllerCommand");
m_rearRightController = requireNonNullParam(rearRightController,
"rearRightController", "MecanumControllerCommand");
m_frontLeftController =
requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand");
m_rearLeftController =
requireNonNullParam(rearLeftController, "rearLeftController", "MecanumControllerCommand");
m_frontRightController =
requireNonNullParam(
frontRightController, "frontRightController", "MecanumControllerCommand");
m_rearRightController =
requireNonNullParam(rearRightController, "rearRightController", "MecanumControllerCommand");
m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
"currentWheelSpeeds", "MecanumControllerCommand");
m_currentWheelSpeeds =
requireNonNullParam(currentWheelSpeeds, "currentWheelSpeeds", "MecanumControllerCommand");
m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
"outputDriveVoltages", "MecanumControllerCommand");
m_outputDriveVoltages =
requireNonNullParam(outputDriveVoltages, "outputDriveVoltages", "MecanumControllerCommand");
m_outputWheelSpeeds = null;
@@ -165,56 +154,64 @@ public class MecanumControllerCommand extends CommandBase {
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
* <p>Note 2: The final rotation of the robot will be set to the rotation of
* the final pose in the trajectory. The robot will not follow the rotations
* from the poses at each timestep. If alternate rotation behavior is desired,
* the other constructor with a supplier for rotation should be used.
* <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
* trajectory. The robot will not follow the rotations from the poses at each timestep. If
* alternate rotation behavior is desired, the other constructor with a supplier for rotation
* should be used.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param feedforward The feedforward to use for the drivetrain.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller
* for the robot's x position.
* @param yController The Trajectory Tracker PID controller
* for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller
* for angle for the robot.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param feedforward The feedforward to use for the drivetrain.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param frontLeftController The front left wheel velocity PID.
* @param rearLeftController The rear left wheel velocity PID.
* @param frontRightController The front right wheel velocity PID.
* @param rearRightController The rear right wheel velocity PID.
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
* the current wheel speeds.
* @param outputDriveVoltages A MecanumDriveMotorVoltages object containing
* the output motor voltages.
* @param requirements The subsystems to require.
* @param frontLeftController The front left wheel velocity PID.
* @param rearLeftController The rear left wheel velocity PID.
* @param frontRightController The front right wheel velocity PID.
* @param rearRightController The rear right wheel velocity PID.
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
* @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
* voltages.
* @param requirements The subsystems to require.
*/
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
public MecanumControllerCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
SimpleMotorFeedforward feedforward,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
PIDController rearRightController,
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
Subsystem... requirements) {
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SimpleMotorFeedforward feedforward,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
PIDController rearRightController,
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
Subsystem... requirements) {
this(trajectory, pose, feedforward, kinematics, xController, yController, thetaController,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1)
.poseMeters.getRotation(),
maxWheelVelocityMetersPerSecond, frontLeftController, rearLeftController,
frontRightController, rearRightController, currentWheelSpeeds, outputDriveVoltages,
this(
trajectory,
pose,
feedforward,
kinematics,
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
maxWheelVelocityMetersPerSecond,
frontLeftController,
rearLeftController,
frontRightController,
rearRightController,
currentWheelSpeeds,
outputDriveVoltages,
requirements);
}
@@ -225,56 +222,50 @@ public class MecanumControllerCommand extends CommandBase {
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
* this is left to the user, since it is not appropriate for paths with non-stationary end-states.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller
* for the robot's x position.
* @param yController The Trajectory Tracker PID controller
* for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller
* for angle for the robot.
* @param desiredRotation The angle that the robot should be facing. This
* is sampled at each time step.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param desiredRotation The angle that the robot should be facing. This is sampled at each time
* step.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing
* the output wheel speeds.
* @param requirements The subsystems to require.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
public MecanumControllerCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory",
"MecanumControllerCommand");
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
m_kinematics = requireNonNullParam(kinematics,
"kinematics", "MecanumControllerCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
m_controller = new HolonomicDriveController(
requireNonNullParam(xController,
"xController", "SwerveControllerCommand"),
requireNonNullParam(yController,
"xController", "SwerveControllerCommand"),
requireNonNullParam(thetaController,
"thetaController", "SwerveControllerCommand")
);
m_controller =
new HolonomicDriveController(
requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
"MecanumControllerCommand");
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
"maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
m_maxWheelVelocityMetersPerSecond =
requireNonNullParam(
maxWheelVelocityMetersPerSecond,
"maxWheelVelocityMetersPerSecond",
"MecanumControllerCommand");
m_frontLeftController = null;
m_rearLeftController = null;
@@ -283,8 +274,8 @@ public class MecanumControllerCommand extends CommandBase {
m_currentWheelSpeeds = null;
m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
"outputWheelSpeeds", "MecanumControllerCommand");
m_outputWheelSpeeds =
requireNonNullParam(outputWheelSpeeds, "outputWheelSpeeds", "MecanumControllerCommand");
m_outputDriveVoltages = null;
@@ -300,54 +291,58 @@ public class MecanumControllerCommand extends CommandBase {
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
* this is left to the user, since it is not appropriate for paths with non-stationary end-states.
*
* <p>Note 2: The final rotation of the robot will be set to the rotation of
* the final pose in the trajectory. The robot will not follow the rotations
* from the poses at each timestep. If alternate rotation behavior is desired,
* the other constructor with a supplier for rotation should be used.
* <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
* trajectory. The robot will not follow the rotations from the poses at each timestep. If
* alternate rotation behavior is desired, the other constructor with a supplier for rotation
* should be used.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller
* for the robot's x position.
* @param yController The Trajectory Tracker PID controller
* for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller
* for angle for the robot.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing
* the output wheel speeds.
* @param requirements The subsystems to require.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
public MecanumControllerCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
this(trajectory, pose, kinematics, xController, yController, thetaController,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1)
.poseMeters.getRotation(),
maxWheelVelocityMetersPerSecond, outputWheelSpeeds, requirements);
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
this(
trajectory,
pose,
kinematics,
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
maxWheelVelocityMetersPerSecond,
outputWheelSpeeds,
requirements);
}
@Override
public void initialize() {
var initialState = m_trajectory.sample(0);
var initialXVelocity = initialState.velocityMetersPerSecond
* initialState.poseMeters.getRotation().getCos();
var initialYVelocity = initialState.velocityMetersPerSecond
* initialState.poseMeters.getRotation().getSin();
var initialXVelocity =
initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
var initialYVelocity =
initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
m_prevSpeeds = m_kinematics.toWheelSpeeds(
new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
m_prevSpeeds =
m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
m_timer.reset();
m_timer.start();
@@ -361,8 +356,8 @@ public class MecanumControllerCommand extends CommandBase {
var desiredState = m_trajectory.sample(curTime);
var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
m_desiredRotation.get());
var targetChassisSpeeds =
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
@@ -378,46 +373,57 @@ public class MecanumControllerCommand extends CommandBase {
double rearRightOutput;
if (m_usePID) {
final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
final double frontLeftFeedforward =
m_feedforward.calculate(
frontLeftSpeedSetpoint,
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
final double rearLeftFeedforward =
m_feedforward.calculate(
rearLeftSpeedSetpoint,
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
(frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
final double frontRightFeedforward =
m_feedforward.calculate(
frontRightSpeedSetpoint,
(frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
(rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
final double rearRightFeedforward =
m_feedforward.calculate(
rearRightSpeedSetpoint,
(rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
frontLeftSpeedSetpoint);
frontLeftOutput =
frontLeftFeedforward
+ m_frontLeftController.calculate(
m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
rearLeftSpeedSetpoint);
rearLeftOutput =
rearLeftFeedforward
+ m_rearLeftController.calculate(
m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
m_currentWheelSpeeds.get().frontRightMetersPerSecond,
frontRightSpeedSetpoint);
frontRightOutput =
frontRightFeedforward
+ m_frontRightController.calculate(
m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
m_currentWheelSpeeds.get().rearRightMetersPerSecond,
rearRightSpeedSetpoint);
rearRightOutput =
rearRightFeedforward
+ m_rearRightController.calculate(
m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
frontLeftOutput,
frontRightOutput,
rearLeftOutput,
rearRightOutput));
m_outputDriveVoltages.accept(
new MecanumDriveMotorVoltages(
frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
} else {
m_outputWheelSpeeds.accept(new MecanumDriveWheelSpeeds(
frontLeftSpeedSetpoint,
frontRightSpeedSetpoint,
rearLeftSpeedSetpoint,
rearRightSpeedSetpoint));
m_outputWheelSpeeds.accept(
new MecanumDriveWheelSpeeds(
frontLeftSpeedSetpoint,
frontRightSpeedSetpoint,
rearLeftSpeedSetpoint,
rearRightSpeedSetpoint));
}
m_prevTime = curTime;

16
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
View File

@@ -4,18 +4,16 @@
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.
* 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(java.util.function.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.
* 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;
@@ -24,8 +22,8 @@ public class NotifierCommand extends CommandBase {
/**
* 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 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) {

51
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
View File

@@ -4,18 +4,17 @@
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.controller.PIDController;
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.
* 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;
@@ -26,15 +25,18 @@ public class PIDCommand extends CommandBase {
/**
* Creates a new PIDCommand, which controls the given output with a PIDController.
*
* @param controller the controller that controls the output.
* @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
* @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) {
public PIDCommand(
PIDController controller,
DoubleSupplier measurementSource,
DoubleSupplier setpointSource,
DoubleConsumer useOutput,
Subsystem... requirements) {
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
@@ -50,15 +52,18 @@ public class PIDCommand extends CommandBase {
/**
* Creates a new PIDCommand, which controls the given output with a PIDController.
*
* @param controller the controller that controls the output.
* @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
* @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) {
public PIDCommand(
PIDController controller,
DoubleSupplier measurementSource,
double setpoint,
DoubleConsumer useOutput,
Subsystem... requirements) {
this(controller, measurementSource, () -> setpoint, useOutput, requirements);
}
@@ -69,8 +74,8 @@ public class PIDCommand extends CommandBase {
@Override
public void execute() {
m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
m_setpoint.getAsDouble()));
m_useOutput.accept(
m_controller.calculate(m_measurement.getAsDouble(), m_setpoint.getAsDouble()));
}
@Override

16
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.controller.PIDController;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.controller.PIDController;
/**
* A subsystem that uses a {@link PIDController} to control an output. The controller is run
* 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 {
@@ -31,7 +31,7 @@ public abstract class PIDSubsystem extends SubsystemBase {
}
/**
* Creates a new PIDSubsystem. Initial setpoint is zero.
* Creates a new PIDSubsystem. Initial setpoint is zero.
*
* @param controller the PIDController to use
*/
@@ -83,17 +83,13 @@ public abstract class PIDSubsystem extends SubsystemBase {
*/
protected abstract double getMeasurement();
/**
* Enables the PID control. Resets the controller.
*/
/** Enables the PID control. Resets the controller. */
public void enable() {
m_enabled = true;
m_controller.reset();
}
/**
* Disables the PID control. Sets output to zero.
*/
/** Disables the PID control. Sets output to zero. */
public void disable() {
m_enabled = false;
useOutput(0, 0);

12
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
View File

@@ -14,14 +14,14 @@ import java.util.Map;
* <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
// 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.
* 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.
*/
@@ -42,8 +42,8 @@ public class ParallelCommandGroup extends CommandGroupBase {
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");
throw new IllegalArgumentException(
"Multiple commands in a parallel group cannot" + "require the same subsystems");
}
m_commands.put(command, false);
m_requirements.addAll(command.getRequirements());

22
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
View File

@@ -9,23 +9,23 @@ 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.
* 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
// 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 boolean m_finished = 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.
* 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
@@ -39,8 +39,8 @@ public class ParallelDeadlineGroup extends CommandGroupBase {
}
/**
* Sets the deadline to the given command. The deadline is added to the group if it is not
* already contained.
* 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
*/
@@ -64,8 +64,8 @@ public class ParallelDeadlineGroup extends CommandGroupBase {
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");
throw new IllegalArgumentException(
"Multiple commands in a parallel group cannot" + "require the same subsystems");
}
m_commands.put(command, false);
m_requirements.addAll(command.getRequirements());

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
View File

@@ -20,8 +20,8 @@ public class ParallelRaceGroup extends CommandGroupBase {
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
* 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.
@@ -43,8 +43,8 @@ public class ParallelRaceGroup extends CommandGroupBase {
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");
throw new IllegalArgumentException(
"Multiple commands in a parallel group cannot" + " require the same subsystems");
}
m_commands.add(command);
m_requirements.addAll(command.getRequirements());

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
View File

@@ -8,7 +8,7 @@ import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCom
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
/**
* A command that runs another command in perpetuity, ignoring that command's end conditions. While
* 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.
@@ -19,8 +19,8 @@ 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.
* 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
*/

4
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
View File

@@ -4,9 +4,7 @@
package edu.wpi.first.wpilibj2.command;
/**
* A command that prints a string when initialized.
*/
/** A command that prints a string when initialized. */
public class PrintCommand extends InstantCommand {
/**
* Creates a new a PrintCommand.

95
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
View File

@@ -4,20 +4,18 @@
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
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
* 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 {
@@ -27,18 +25,21 @@ public class ProfiledPIDCommand extends CommandBase {
protected BiConsumer<Double, State> m_useOutput;
/**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
* Goal velocity is specified.
* 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 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
* @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) {
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");
@@ -52,18 +53,21 @@ public class ProfiledPIDCommand extends CommandBase {
}
/**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
* Goal velocity is implicitly zero.
* 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 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
* @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) {
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");
@@ -80,15 +84,18 @@ public class ProfiledPIDCommand extends CommandBase {
* 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 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
* @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) {
public ProfiledPIDCommand(
ProfiledPIDController controller,
DoubleSupplier measurementSource,
State goal,
BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
this(controller, measurementSource, () -> goal, useOutput, requirements);
}
@@ -96,15 +103,18 @@ public class ProfiledPIDCommand extends CommandBase {
* 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 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
* @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) {
public ProfiledPIDCommand(
ProfiledPIDController controller,
DoubleSupplier measurementSource,
double goal,
BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
this(controller, measurementSource, () -> goal, useOutput, requirements);
}
@@ -115,8 +125,9 @@ public class ProfiledPIDCommand extends CommandBase {
@Override
public void execute() {
m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
m_controller.getSetpoint());
m_useOutput.accept(
m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
m_controller.getSetpoint());
}
@Override

27
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
View File

@@ -4,15 +4,15 @@
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
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;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
/**
* A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is
* run synchronously from the subsystem's periodic() method.
* 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;
@@ -26,14 +26,13 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
* @param controller the ProfiledPIDController to use
* @param initialPosition the initial goal position of the controller
*/
public ProfiledPIDSubsystem(ProfiledPIDController controller,
double initialPosition) {
public ProfiledPIDSubsystem(ProfiledPIDController controller, double initialPosition) {
m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
setGoal(initialPosition);
}
/**
* Creates a new ProfiledPIDSubsystem. Initial goal position is zero.
* Creates a new ProfiledPIDSubsystem. Initial goal position is zero.
*
* @param controller the ProfiledPIDController to use
*/
@@ -62,7 +61,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
}
/**
* Sets the goal state for the subsystem. Goal velocity assumed to be zero.
* Sets the goal state for the subsystem. Goal velocity assumed to be zero.
*
* @param goal The goal position for the subsystem's motion profile.
*/
@@ -73,7 +72,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
/**
* Uses the output from the ProfiledPIDController.
*
* @param output the output of the ProfiledPIDController
* @param output the output of the ProfiledPIDController
* @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
*/
protected abstract void useOutput(double output, State setpoint);
@@ -85,17 +84,13 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
*/
protected abstract double getMeasurement();
/**
* Enables the PID control. Resets the controller.
*/
/** Enables the PID control. Resets the controller. */
public void enable() {
m_enabled = true;
m_controller.reset(getMeasurement());
}
/**
* Disables the PID control. Sets output to zero.
*/
/** Disables the PID control. Sets output to zero. */
public void disable() {
m_enabled = false;
useOutput(0, new State());

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
View File

@@ -8,7 +8,7 @@ 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,
* 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 {
@@ -16,8 +16,8 @@ public class ProxyScheduleCommand extends CommandBase {
private boolean m_finished;
/**
* Creates a new ProxyScheduleCommand that schedules the given commands when initialized,
* and ends when they are all no longer scheduled.
* 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
*/

139
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -4,8 +4,7 @@
package edu.wpi.first.wpilibj2.command;
import java.util.function.BiConsumer;
import java.util.function.Supplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.controller.PIDController;
@@ -16,21 +15,20 @@ 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;
import java.util.function.BiConsumer;
import java.util.function.Supplier;
/**
* 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
* <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.
* <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 {
@@ -49,39 +47,39 @@ public class RamseteCommand extends CommandBase {
private double m_prevTime;
/**
* Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
* PID control and feedforward are handled internally, and outputs are scaled -12 to 12
* representing units of volts.
* Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID
* control and feedforward are handled internally, and outputs are scaled -12 to 12 representing
* units of volts.
*
* <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.
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param controller The RAMSETE controller used to follow the trajectory.
* @param feedforward The feedforward to use for the drive.
* @param kinematics The kinematics for the robot drivetrain.
* @param wheelSpeeds A function that supplies the speeds of the left and
* right sides of the robot drive.
* @param leftController The PIDController for the left side of the robot drive.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param controller The RAMSETE controller used to follow the trajectory.
* @param feedforward The feedforward to use for the drive.
* @param kinematics The kinematics for the robot drivetrain.
* @param wheelSpeeds A function that supplies the speeds of the left and right sides of the robot
* drive.
* @param leftController The PIDController for the left side of the robot drive.
* @param rightController The PIDController for the right side of the robot drive.
* @param outputVolts A function that consumes the computed left and right
* outputs (in volts) for the robot drive.
* @param requirements The subsystems to require.
* @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,
SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
PIDController leftController,
PIDController rightController,
BiConsumer<Double, Double> outputVolts,
Subsystem... requirements) {
public RamseteCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
RamseteController controller,
SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
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");
@@ -99,24 +97,24 @@ public class RamseteCommand extends CommandBase {
/**
* 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.
* 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.
* @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) {
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");
@@ -137,11 +135,12 @@ public class RamseteCommand extends CommandBase {
public void initialize() {
m_prevTime = -1;
var initialState = m_trajectory.sample(0);
m_prevSpeeds = m_kinematics.toWheelSpeeds(
new ChassisSpeeds(initialState.velocityMetersPerSecond,
0,
initialState.curvatureRadPerMeter
* initialState.velocityMetersPerSecond));
m_prevSpeeds =
m_kinematics.toWheelSpeeds(
new ChassisSpeeds(
initialState.velocityMetersPerSecond,
0,
initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond));
m_timer.reset();
m_timer.start();
if (m_usePID) {
@@ -161,8 +160,9 @@ public class RamseteCommand extends CommandBase {
return;
}
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
var targetWheelSpeeds =
m_kinematics.toWheelSpeeds(
m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
@@ -172,20 +172,21 @@ public class RamseteCommand extends CommandBase {
if (m_usePID) {
double leftFeedforward =
m_feedforward.calculate(leftSpeedSetpoint,
(leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
m_feedforward.calculate(
leftSpeedSetpoint, (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
double rightFeedforward =
m_feedforward.calculate(rightSpeedSetpoint,
(rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
m_feedforward.calculate(
rightSpeedSetpoint, (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
leftOutput = leftFeedforward
+ m_leftController.calculate(m_speeds.get().leftMetersPerSecond,
leftSpeedSetpoint);
leftOutput =
leftFeedforward
+ m_leftController.calculate(m_speeds.get().leftMetersPerSecond, leftSpeedSetpoint);
rightOutput = rightFeedforward
+ m_rightController.calculate(m_speeds.get().rightMetersPerSecond,
rightSpeedSetpoint);
rightOutput =
rightFeedforward
+ m_rightController.calculate(
m_speeds.get().rightMetersPerSecond, rightSpeedSetpoint);
} else {
leftOutput = leftSpeedSetpoint;
rightOutput = rightSpeedSetpoint;

17
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
View File

@@ -4,24 +4,23 @@
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import java.util.function.BooleanSupplier;
/**
* 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}.
* 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.
* 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 toRun the Runnable to run
* @param requirements the subsystems to require
*/
public RunCommand(Runnable toRun, Subsystem... requirements) {

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
View File

@@ -7,9 +7,9 @@ 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.
* 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;

19
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
View File

@@ -4,18 +4,18 @@
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;
import java.util.Map;
import java.util.function.Supplier;
/**
* 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
* 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,
* 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
@@ -39,7 +39,7 @@ public class SelectCommand extends CommandBase {
public SelectCommand(Map<Object, Command> commands, Supplier<Object> selector) {
requireUngrouped(commands.values());
CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{}));
CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[] {}));
m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
@@ -66,8 +66,9 @@ public class SelectCommand extends CommandBase {
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!");
m_selectedCommand =
new PrintCommand(
"SelectCommand selector value does not correspond to" + " any command!");
return;
}
m_selectedCommand = m_commands.get(m_selector.get());

11
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
View File

@@ -18,8 +18,8 @@ public class SequentialCommandGroup extends CommandGroupBase {
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.
* 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.
*/
@@ -74,9 +74,10 @@ public class SequentialCommandGroup extends CommandGroupBase {
@Override
public void end(boolean interrupted) {
if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1
&& m_currentCommandIndex < m_commands.size()
) {
if (interrupted
&& !m_commands.isEmpty()
&& m_currentCommandIndex > -1
&& m_currentCommandIndex < m_commands.size()) {
m_commands.get(m_currentCommandIndex).end(true);
}
m_currentCommandIndex = -1;

16
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
View File

@@ -4,26 +4,24 @@
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 initialized, 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.
* 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(java.util.function.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.
* 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 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) {

54
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
View File

@@ -5,45 +5,43 @@
package edu.wpi.first.wpilibj2.command;
/**
* A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
* 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
* 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
* "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.
* <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.
* 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() {
}
default void periodic() {}
/**
* This method is called periodically by the {@link CommandScheduler}. Useful for updating
* This method is called periodically by the {@link CommandScheduler}. Useful for updating
* subsystem-specific state that needs to be maintained for simulations, such as for updating
* {@link edu.wpi.first.wpilibj.simulation} classes and setting simulated sensor readings.
*/
default void simulationPeriodic() {
}
default void simulationPeriodic() {}
/**
* 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}.
* 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
*/
@@ -52,8 +50,8 @@ public interface Subsystem {
}
/**
* Gets the default command for this subsystem. Returns null if no default command is
* currently associated with the subsystem.
* 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
*/
@@ -62,7 +60,7 @@ public interface Subsystem {
}
/**
* Returns the command currently running on this subsystem. Returns null if no command is
* 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
@@ -72,8 +70,8 @@ public interface Subsystem {
}
/**
* Registers this subsystem with the {@link CommandScheduler}, allowing its
* {@link Subsystem#periodic()} method to be called when the scheduler runs.
* 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);

19
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
View File

@@ -14,9 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
*/
public abstract class SubsystemBase implements Subsystem, Sendable {
/**
* Constructor.
*/
/** Constructor. */
public SubsystemBase() {
String name = this.getClass().getSimpleName();
name = name.substring(name.lastIndexOf('.') + 1);
@@ -65,8 +63,7 @@ public abstract class SubsystemBase implements Subsystem, Sendable {
}
/**
* Associates a {@link Sendable} with this Subsystem.
* Also update the child's name.
* Associates a {@link Sendable} with this Subsystem. Also update the child's name.
*
* @param name name to give child
* @param child sendable
@@ -80,10 +77,14 @@ public abstract class SubsystemBase implements Subsystem, Sendable {
builder.setSmartDashboardType("Subsystem");
builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
builder.addStringProperty(".default",
() -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", 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);
builder.addStringProperty(
".command",
() -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none",
null);
}
}

155
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
View File

@@ -4,8 +4,7 @@
package edu.wpi.first.wpilibj2.command;
import java.util.function.Consumer;
import java.util.function.Supplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
@@ -16,22 +15,20 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import java.util.function.Consumer;
import java.util.function.Supplier;
/**
* A command that uses two PID controllers ({@link PIDController}) and a
* ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
* {@link Trajectory} with a swerve drive.
* A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
* ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a swerve drive.
*
* <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState})
* in an array. The desired wheel and module rotation velocities should be taken
* from those and used in velocity PIDs.
* <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState}) in an
* array. The desired wheel and module rotation velocities should be taken from those and used in
* velocity PIDs.
*
* <p>The robot angle controller does not follow the angle given by
* the trajectory but rather goes to the angle given in the final state of the trajectory.
* <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
* to the angle given in the final state of the trajectory.
*/
@SuppressWarnings("MemberName")
public class SwerveControllerCommand extends CommandBase {
private final Timer m_timer = new Timer();
@@ -50,50 +47,44 @@ public class SwerveControllerCommand extends CommandBase {
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller
* for the robot's x position.
* @param yController The Trajectory Tracker PID controller
* for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller
* for angle for the robot.
* @param desiredRotation The angle that the drivetrain should be facing. This
* is sampled at each time step.
* @param outputModuleStates The raw output module states from the
* position controllers.
* @param requirements The subsystems to require.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each
* time step.
* @param outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
@SuppressWarnings("ParameterName")
public SwerveControllerCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
m_controller = new HolonomicDriveController(
requireNonNullParam(xController,
"xController", "SwerveControllerCommand"),
requireNonNullParam(yController,
"xController", "SwerveControllerCommand"),
requireNonNullParam(thetaController,
"thetaController", "SwerveControllerCommand")
);
m_controller =
new HolonomicDriveController(
requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
m_outputModuleStates = requireNonNullParam(outputModuleStates,
"frontLeftOutput", "SwerveControllerCommand");
m_outputModuleStates =
requireNonNullParam(outputModuleStates, "frontLeftOutput", "SwerveControllerCommand");
m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
"SwerveControllerCommand");
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
addRequirements(requirements);
}
@@ -106,38 +97,42 @@ public class SwerveControllerCommand extends CommandBase {
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
* <p>Note 2: The final rotation of the robot will be set to the rotation of
* the final pose in the trajectory. The robot will not follow the rotations
* from the poses at each timestep. If alternate rotation behavior is desired,
* the other constructor with a supplier for rotation should be used.
* <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
* trajectory. The robot will not follow the rotations from the poses at each timestep. If
* alternate rotation behavior is desired, the other constructor with a supplier for rotation
* should be used.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller
* for the robot's x position.
* @param yController The Trajectory Tracker PID controller
* for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller
* for angle for the robot.
* @param outputModuleStates The raw output module states from the
* position controllers.
* @param requirements The subsystems to require.
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
@SuppressWarnings("ParameterName")
public SwerveControllerCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
this(trajectory, pose, kinematics, xController, yController, thetaController,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1)
.poseMeters.getRotation(),
outputModuleStates, requirements);
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
this(
trajectory,
pose,
kinematics,
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
outputModuleStates,
requirements);
}
@Override
@@ -152,8 +147,8 @@ public class SwerveControllerCommand extends CommandBase {
double curTime = m_timer.get();
var desiredState = m_trajectory.sample(curTime);
var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
m_desiredRotation.get());
var targetChassisSpeeds =
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
m_outputModuleStates.accept(targetModuleStates);

21
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
View File

@@ -4,17 +4,15 @@
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;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import java.util.function.Consumer;
/**
* A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism
* motion.
* A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion.
*/
public class TrapezoidProfileCommand extends CommandBase {
private final TrapezoidProfile m_profile;
@@ -26,13 +24,12 @@ public class TrapezoidProfileCommand extends CommandBase {
* 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 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) {
public TrapezoidProfileCommand(
TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
addRequirements(requirements);

43
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
View File

@@ -4,13 +4,13 @@
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
/**
* A subsystem that generates and runs trapezoidal motion profiles automatically. The user
* specifies how to use the current state of the motion profile by overriding the `useState` method.
* A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies
* how to use the current state of the motion profile by overriding the `useState` method.
*/
public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
private final double m_period;
@@ -24,14 +24,13 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
/**
* Creates a new TrapezoidProfileSubsystem.
*
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
* @param initialPosition The initial position of the controlled mechanism when the subsystem
* is constructed.
* @param period The period of the main robot loop, in seconds.
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
* @param initialPosition The initial position of the controlled mechanism when the subsystem is
* constructed.
* @param period The period of the main robot loop, in seconds.
*/
public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
double initialPosition,
double period) {
public TrapezoidProfileSubsystem(
TrapezoidProfile.Constraints constraints, double initialPosition, double period) {
m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
m_state = new TrapezoidProfile.State(initialPosition, 0);
setGoal(initialPosition);
@@ -41,19 +40,19 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
/**
* Creates a new TrapezoidProfileSubsystem.
*
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
* @param initialPosition The initial position of the controlled mechanism when the subsystem
* is constructed.
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
* @param initialPosition The initial position of the controlled mechanism when the subsystem is
* constructed.
*/
public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
double initialPosition) {
public TrapezoidProfileSubsystem(
TrapezoidProfile.Constraints constraints, double initialPosition) {
this(constraints, initialPosition, 0.02);
}
/**
* Creates a new TrapezoidProfileSubsystem.
*
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
*/
public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
this(constraints, 0, 0.02);
@@ -78,7 +77,7 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
}
/**
* Sets the goal state for the subsystem. Goal velocity assumed to be zero.
* Sets the goal state for the subsystem. Goal velocity assumed to be zero.
*
* @param goal The goal position for the subsystem's motion profile.
*/
@@ -86,16 +85,12 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
setGoal(new TrapezoidProfile.State(goal, 0));
}
/**
* Enable the TrapezoidProfileSubsystem's output.
*/
/** Enable the TrapezoidProfileSubsystem's output. */
public void enable() {
m_enabled = true;
}
/**
* Disable the TrapezoidProfileSubsystem's output.
*/
/** Disable the TrapezoidProfileSubsystem's output. */
public void disable() {
m_enabled = false;
}

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
View File

@@ -8,15 +8,15 @@ 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.
* 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.
* Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
*
* @param seconds the time to wait, in seconds
*/

14
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
View File

@@ -4,15 +4,13 @@
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;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.BooleanSupplier;
/**
* A command that does nothing but ends after a specified match time or condition. Useful for
* A command that does nothing but ends after a specified match time or condition. Useful for
* CommandGroups.
*/
public class WaitUntilCommand extends CommandBase {
@@ -30,9 +28,9 @@ public class WaitUntilCommand extends CommandBase {
/**
* 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
* <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.
* referees. When in doubt, add a safety factor or time the action manually.
*
* @param time the match time after which to end, in seconds
*/

43
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
View File

@@ -4,16 +4,15 @@
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.Subsystem;
import java.util.function.BooleanSupplier;
/**
* 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>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
@@ -24,8 +23,7 @@ public class Button extends Trigger {
* Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
* overridden).
*/
public Button() {
}
public Button() {}
/**
* Creates a new button with the given condition determining whether it is pressed.
@@ -39,7 +37,7 @@ public class Button extends Trigger {
/**
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
@@ -63,7 +61,7 @@ public class Button extends Trigger {
/**
* Runs the given runnable whenever the button is newly pressed.
*
* @param toRun the runnable to run
* @param toRun the runnable to run
* @param requirements the required subsystems
* @return this button, so calls can be chained
*/
@@ -75,10 +73,10 @@ public class Button extends Trigger {
/**
* 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.
* <p>{@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 command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
@@ -90,8 +88,8 @@ public class Button extends Trigger {
/**
* 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.
* <p>{@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
@@ -104,7 +102,7 @@ public class Button extends Trigger {
/**
* Constantly runs the given runnable while the button is held.
*
* @param toRun the runnable to run
* @param toRun the runnable to run
* @param requirements the required subsystems
* @return this button, so calls can be chained
*/
@@ -117,7 +115,7 @@ public class Button extends Trigger {
* 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 command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
@@ -128,7 +126,7 @@ public class Button extends Trigger {
/**
* 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
* 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
@@ -139,11 +137,10 @@ public class Button extends Trigger {
return this;
}
/**
* Starts the command when the button is released.
*
* @param command the command to start
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
@@ -153,7 +150,7 @@ public class Button extends Trigger {
}
/**
* Starts the command when the button is released. The command is set to be interruptible.
* 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
@@ -166,7 +163,7 @@ public class Button extends Trigger {
/**
* Runs the given runnable when the button is released.
*
* @param toRun the runnable to run
* @param toRun the runnable to run
* @param requirements the required subsystems
* @return this button, so calls can be chained
*/
@@ -178,7 +175,7 @@ public class Button extends Trigger {
/**
* Toggles the command whenever the button is pressed (on then off then on).
*
* @param command the command to start
* @param command the command to start
* @param interruptible whether the command is interruptible
*/
public Button toggleWhenPressed(final Command command, boolean interruptible) {
@@ -187,8 +184,8 @@ public class Button extends Trigger {
}
/**
* Toggles the command whenever the button is pressed (on then off then on). The command is set
* to be interruptible.
* 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

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
View File

@@ -12,9 +12,7 @@ public class InternalButton extends Button {
private boolean m_pressed;
private boolean m_inverted;
/**
* Creates an InternalButton that is not inverted.
*/
/** Creates an InternalButton that is not inverted. */
public InternalButton() {
this(false);
}
@@ -23,7 +21,7 @@ public class InternalButton extends Button {
* 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.
* when set to false.
*/
public InternalButton(boolean inverted) {
m_pressed = m_inverted = inverted;

11
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
View File

@@ -4,13 +4,11 @@
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}.
*/
import edu.wpi.first.wpilibj.GenericHID;
/** 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;
@@ -18,8 +16,7 @@ public class JoystickButton extends Button {
/**
* Creates a joystick button for triggering commands.
*
* @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick,
* etc)
* @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) {

4
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java
View File

@@ -8,9 +8,7 @@ import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
/**
* A {@link Button} that uses a {@link NetworkTable} boolean field.
*/
/** A {@link Button} that uses a {@link NetworkTable} boolean field. */
public class NetworkButton extends Button {
private final NetworkTableEntry m_entry;

11
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
View File

@@ -4,13 +4,11 @@
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}.
*/
import edu.wpi.first.wpilibj.GenericHID;
/** 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;
@@ -32,8 +30,7 @@ public class POVButton extends Button {
}
/**
* Creates a POV button for triggering commands.
* By default, acts on POV 0
* 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)

209
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
View File

@@ -4,25 +4,24 @@
package edu.wpi.first.wpilibj2.command.button;
import java.util.function.BooleanSupplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.Subsystem;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import java.util.function.BooleanSupplier;
/**
* 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 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.
* <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.
*/
public class Trigger {
private final BooleanSupplier m_isActive;
@@ -37,7 +36,7 @@ public class Trigger {
}
/**
* Creates a new trigger that is always inactive. Useful only as a no-arg constructor for
* 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() {
@@ -58,33 +57,35 @@ public class Trigger {
/**
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
* @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();
CommandScheduler.getInstance()
.addButton(
new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
command.schedule(interruptible);
}
if (!m_pressedLast && pressed) {
command.schedule(interruptible);
}
m_pressedLast = pressed;
}
});
m_pressedLast = pressed;
}
});
return this;
}
/**
* Starts the given command whenever the trigger just becomes active. The command is set to be
* Starts the given command whenever the trigger just becomes active. The command is set to be
* interruptible.
*
* @param command the command to start
@@ -97,7 +98,7 @@ public class Trigger {
/**
* Runs the given runnable whenever the trigger just becomes active.
*
* @param toRun the runnable to run
* @param toRun the runnable to run
* @param requirements the required subsystems
* @return this trigger, so calls can be chained
*/
@@ -108,40 +109,42 @@ public class Trigger {
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* <p>{@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 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();
CommandScheduler.getInstance()
.addButton(
new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
@Override
public void run() {
boolean pressed = get();
if (pressed) {
command.schedule(interruptible);
} else if (m_pressedLast) {
command.cancel();
}
if (pressed) {
command.schedule(interruptible);
} else if (m_pressedLast) {
command.cancel();
}
m_pressedLast = pressed;
}
});
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.
* <p>{@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
@@ -153,7 +156,7 @@ public class Trigger {
/**
* Constantly runs the given runnable while the button is held.
*
* @param toRun the runnable to run
* @param toRun the runnable to run
* @param requirements the required subsystems
* @return this trigger, so calls can be chained
*/
@@ -165,35 +168,37 @@ public class Trigger {
* 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 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();
CommandScheduler.getInstance()
.addButton(
new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
command.schedule(interruptible);
} else if (m_pressedLast && !pressed) {
command.cancel();
}
if (!m_pressedLast && pressed) {
command.schedule(interruptible);
} else if (m_pressedLast && !pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
});
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.
* 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
@@ -205,32 +210,34 @@ public class Trigger {
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
* @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();
CommandScheduler.getInstance()
.addButton(
new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
@Override
public void run() {
boolean pressed = get();
if (m_pressedLast && !pressed) {
command.schedule(interruptible);
}
if (m_pressedLast && !pressed) {
command.schedule(interruptible);
}
m_pressedLast = pressed;
}
});
m_pressedLast = pressed;
}
});
return this;
}
/**
* Starts the command when the trigger becomes inactive. The command is set to be interruptible.
* 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
@@ -242,7 +249,7 @@ public class Trigger {
/**
* Runs the given runnable when the trigger becomes inactive.
*
* @param toRun the runnable to run
* @param toRun the runnable to run
* @param requirements the required subsystems
* @return this trigger, so calls can be chained
*/
@@ -253,36 +260,38 @@ public class Trigger {
/**
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
* @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();
CommandScheduler.getInstance()
.addButton(
new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
if (command.isScheduled()) {
command.cancel();
} else {
command.schedule(interruptible);
}
}
if (!m_pressedLast && pressed) {
if (command.isScheduled()) {
command.cancel();
} else {
command.schedule(interruptible);
}
}
m_pressedLast = pressed;
}
});
m_pressedLast = pressed;
}
});
return this;
}
/**
* Toggles a command when the trigger becomes active. The command is set to be interruptible.
* 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
@@ -300,20 +309,22 @@ public class Trigger {
public Trigger cancelWhenActive(final Command command) {
requireNonNullParam(command, "command", "cancelWhenActive");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
CommandScheduler.getInstance()
.addButton(
new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
command.cancel();
}
if (!m_pressedLast && pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
});
m_pressedLast = pressed;
}
});
return this;
}