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

Split the two command implementations into separate libraries (#2012)

Browse Source 
This will allow us at the user code side to determine to include old commands, new commands or both.
This commit is contained in:
Thad House
2019-11-01 21:58:54 -07:00
committed by Peter Johnson
parent 2ad15cae19
commit 509819d83f
271 changed files with 470 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
View File

@@ -1,70 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.wpilibj.command.Command;
/**
* This class provides an easy way to link commands to OI inputs.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger button
* of a joystick to a "score" command.
*
* <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
* operator interface as a common use case of the more generalized Trigger objects. This is a simple
* wrapper around Trigger with the method names renamed to fit the Button object use.
*/
public abstract class Button extends Trigger {
/**
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
*/
public void whenPressed(final Command command) {
whenActive(command);
}
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#start()} will be called repeatedly while the button is held, and will be
* canceled when the button is released.
*
* @param command the command to start
*/
public void whileHeld(final Command command) {
whileActive(command);
}
/**
* Starts the command when the button is released.
*
* @param command the command to start
*/
public void whenReleased(final Command command) {
whenInactive(command);
}
/**
* Toggles the command whenever the button is pressed (on then off then on).
*
* @param command the command to start
*/
public void toggleWhenPressed(final Command command) {
toggleWhenActive(command);
}
/**
* Cancel the command when the button is pressed.
*
* @param command the command to start
*/
public void cancelWhenPressed(final Command command) {
cancelWhenActive(command);
}
}

47
wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
View File

@@ -1,47 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.buttons;
/**
* This class is intended to be used within a program. The programmer can manually set its value.
* Also includes a setting for whether or not it should invert its value.
*/
public class InternalButton extends Button {
private boolean m_pressed;
private boolean m_inverted;
/**
* Creates an InternalButton that is not inverted.
*/
public InternalButton() {
this(false);
}
/**
* Creates an InternalButton which is inverted depending on the input.
*
* @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
* when set to false.
*/
public InternalButton(boolean inverted) {
m_pressed = m_inverted = inverted;
}
public void setInverted(boolean inverted) {
m_inverted = inverted;
}
public void setPressed(boolean pressed) {
m_pressed = pressed;
}
@Override
public boolean get() {
return m_pressed ^ m_inverted;
}
}

40
wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
View File

@@ -1,40 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.buttons;
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;
/**
* Create a joystick button for triggering commands.
*
* @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick,
* etc)
* @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
*/
public JoystickButton(GenericHID joystick, int buttonNumber) {
m_joystick = joystick;
m_buttonNumber = buttonNumber;
}
/**
* Gets the value of the joystick button.
*
* @return The value of the joystick button
*/
@Override
public boolean get() {
return m_joystick.getRawButton(m_buttonNumber);
}
}

32
wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.buttons;
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.
*/
public class NetworkButton extends Button {
private final NetworkTableEntry m_entry;
public NetworkButton(String table, String field) {
this(NetworkTableInstance.getDefault().getTable(table), field);
}
public NetworkButton(NetworkTable table, String field) {
m_entry = table.getEntry(field);
}
@Override
public boolean get() {
return m_entry.getInstance().isConnected() && m_entry.getBoolean(false);
}
}

53
wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
View File

@@ -1,53 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.buttons;
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;
private final int m_povNumber;
/**
* Creates a POV button for triggering commands.
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle in degrees (e.g. 90, 270)
* @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
*/
public POVButton(GenericHID joystick, int angle, int povNumber) {
m_joystick = joystick;
m_angle = angle;
m_povNumber = povNumber;
}
/**
* Creates a POV button for triggering commands.
* By default, acts on POV 0
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle (e.g. 90, 270)
*/
public POVButton(GenericHID joystick, int angle) {
this(joystick, angle, 0);
}
/**
* Checks whether the current value of the POV is the target angle.
*
* @return Whether the value of the POV matches the target angle
*/
@Override
public boolean get() {
return m_joystick.getPOV(m_povNumber) == m_angle;
}
}

185
wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
View File

@@ -1,185 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.buttons;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
/**
* This class provides an easy way to link commands to inputs.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger
* button of a joystick to a "score" command.
*
* <p>It is encouraged that teams write a subclass of Trigger if they want to have something unusual
* (for instance, if they want to react to the user holding a button while the robot is reading a
* certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get
* the full functionality of the Trigger class.
*/
public abstract class Trigger implements Sendable {
private volatile boolean m_sendablePressed;
/**
* Returns whether or not the trigger is active.
*
* <p>This method will be called repeatedly a command is linked to the Trigger.
*
* @return whether or not the trigger condition is active.
*/
public abstract boolean get();
/**
* Returns whether get() return true or the internal table for SmartDashboard use is pressed.
*
* @return whether get() return true or the internal table for SmartDashboard use is pressed.
*/
@SuppressWarnings("PMD.UselessParentheses")
private boolean grab() {
return get() || m_sendablePressed;
}
/**
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
*/
public void whenActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (!m_pressedLast && pressed) {
command.start();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#start()} will be called repeatedly while the trigger is active, and will be
* canceled when the trigger becomes inactive.
*
* @param command the command to start
*/
public void whileActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (pressed) {
command.start();
} else if (m_pressedLast && !pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
*/
public void whenInactive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (m_pressedLast && !pressed) {
command.start();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
*/
public void toggleWhenActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (!m_pressedLast && pressed) {
if (command.isRunning()) {
command.cancel();
} else {
command.start();
}
}
m_pressedLast = pressed;
}
}.start();
}
/**
* Cancels a command when the trigger becomes active.
*
* @param command the command to cancel
*/
public void cancelWhenActive(final Command command) {
new ButtonScheduler() {
private boolean m_pressedLast = grab();
@Override
public void execute() {
boolean pressed = grab();
if (!m_pressedLast && pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
}.start();
}
/**
* An internal class of {@link Trigger}. The user should ignore this, it is only public to
* interface between packages.
*/
public abstract static class ButtonScheduler {
public abstract void execute();
public void start() {
Scheduler.getInstance().addButton(this);
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Button");
builder.setSafeState(() -> m_sendablePressed = false);
builder.addBooleanProperty("pressed", this::grab, value -> m_sendablePressed = value);
}
}

670
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java
View File

@@ -1,670 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* The Command class is at the very core of the entire command framework. Every command can be
* started with a call to {@link Command#start() start()}. Once a command is started it will call
* {@link Command#initialize() initialize()}, and then will repeatedly call {@link Command#execute()
* execute()} until the {@link Command#isFinished() isFinished()} returns true. Once it does, {@link
* Command#end() end()} will be called.
*
* <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called,
* then the command will be stopped and {@link Command#interrupted() interrupted()} will be called.
*
* <p>If a command uses a {@link Subsystem}, then it should specify that it does so by calling the
* {@link Command#requires(Subsystem) requires(...)} method in its constructor. Note that a Command
* may have multiple requirements, and {@link Command#requires(Subsystem) requires(...)} should be
* called for each one.
*
* <p>If a command is running and a new command with shared requirements is started, then one of
* two things will happen. If the active command is interruptible, then {@link Command#cancel()
* cancel()} will be called and the command will be removed to make way for the new one. If the
* active command is not interruptible, the other one will not even be started, and the active one
* will continue functioning.
*
* @see Subsystem
* @see CommandGroup
* @see IllegalUseOfCommandException
*/
@SuppressWarnings("PMD.TooManyMethods")
public abstract class Command implements Sendable, AutoCloseable {
/**
* The time since this command was initialized.
*/
private double m_startTime = -1;
/**
* The time (in seconds) before this command "times out" (or -1 if no timeout).
*/
private double m_timeout = -1;
/**
* Whether or not this command has been initialized.
*/
private boolean m_initialized;
/**
* The required subsystems.
*/
private final Set m_requirements = new Set();
/**
* Whether or not it is running.
*/
private boolean m_running;
/**
* Whether or not it is interruptible.
*/
private boolean m_interruptible = true;
/**
* Whether or not it has been canceled.
*/
private boolean m_canceled;
/**
* Whether or not it has been locked.
*/
private boolean m_locked;
/**
* Whether this command should run when the robot is disabled.
*/
private boolean m_runWhenDisabled;
/**
* Whether or not this command has completed running.
*/
private boolean m_completed;
/**
* The {@link CommandGroup} this is in.
*/
private CommandGroup m_parent;
/**
* Creates a new command. The name of this command will be set to its class name.
*/
public Command() {
String name = getClass().getName();
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
}
/**
* Creates a new command with the given name.
*
* @param name the name for this command
* @throws IllegalArgumentException if name is null
*/
public Command(String name) {
if (name == null) {
throw new IllegalArgumentException("Name must not be null.");
}
SendableRegistry.add(this, name);
}
/**
* Creates a new command with the given timeout and a default name. The default name is the name
* of the class.
*
* @param timeout the time (in seconds) before this command "times out"
* @throws IllegalArgumentException if given a negative timeout
* @see Command#isTimedOut() isTimedOut()
*/
public Command(double timeout) {
this();
if (timeout < 0) {
throw new IllegalArgumentException("Timeout must not be negative. Given:" + timeout);
}
m_timeout = timeout;
}
/**
* Creates a new command with the given timeout and a default name. The default name is the name
* of the class.
*
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if given a negative timeout
* @see Command#isTimedOut() isTimedOut()
*/
public Command(Subsystem subsystem) {
this();
requires(subsystem);
}
/**
* Creates a new command with the given name.
*
* @param name the name for this command
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if name is null
*/
public Command(String name, Subsystem subsystem) {
this(name);
requires(subsystem);
}
/**
* Creates a new command with the given timeout and a default name. The default name is the name
* of the class.
*
* @param timeout the time (in seconds) before this command "times out"
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if given a negative timeout
* @see Command#isTimedOut() isTimedOut()
*/
public Command(double timeout, Subsystem subsystem) {
this(timeout);
requires(subsystem);
}
/**
* Creates a new command with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time (in seconds) before this command "times out"
* @throws IllegalArgumentException if given a negative timeout or name was null.
* @see Command#isTimedOut() isTimedOut()
*/
public Command(String name, double timeout) {
this(name);
if (timeout < 0) {
throw new IllegalArgumentException("Timeout must not be negative. Given:" + timeout);
}
m_timeout = timeout;
}
/**
* Creates a new command with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time (in seconds) before this command "times out"
* @param subsystem the subsystem that this command requires
* @throws IllegalArgumentException if given a negative timeout
* @throws IllegalArgumentException if given a negative timeout or name was null.
* @see Command#isTimedOut() isTimedOut()
*/
public Command(String name, double timeout, Subsystem subsystem) {
this(name, timeout);
requires(subsystem);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Sets the timeout of this command.
*
* @param seconds the timeout (in seconds)
* @throws IllegalArgumentException if seconds is negative
* @see Command#isTimedOut() isTimedOut()
*/
protected final synchronized void setTimeout(double seconds) {
if (seconds < 0) {
throw new IllegalArgumentException("Seconds must be positive. Given:" + seconds);
}
m_timeout = seconds;
}
/**
* Returns the time since this command was initialized (in seconds). This function will work even
* if there is no specified timeout.
*
* @return the time since this command was initialized (in seconds).
*/
public final synchronized double timeSinceInitialized() {
return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
}
/**
* This method specifies that the given {@link Subsystem} is used by this command. This method is
* crucial to the functioning of the Command System in general.
*
* <p>Note that the recommended way to call this method is in the constructor.
*
* @param subsystem the {@link Subsystem} required
* @throws IllegalArgumentException if subsystem is null
* @throws IllegalUseOfCommandException if this command has started before or if it has been given
* to a {@link CommandGroup}
* @see Subsystem
*/
protected synchronized void requires(Subsystem subsystem) {
validate("Can not add new requirement to command");
if (subsystem != null) {
m_requirements.add(subsystem);
} else {
throw new IllegalArgumentException("Subsystem must not be null.");
}
}
/**
* Called when the command has been removed. This will call {@link Command#interrupted()
* interrupted()} or {@link Command#end() end()}.
*/
synchronized void removed() {
if (m_initialized) {
if (isCanceled()) {
interrupted();
_interrupted();
} else {
end();
_end();
}
}
m_initialized = false;
m_canceled = false;
m_running = false;
m_completed = true;
}
/**
* The run method is used internally to actually run the commands.
*
* @return whether or not the command should stay within the {@link Scheduler}.
*/
synchronized boolean run() {
if (!m_runWhenDisabled && m_parent == null && RobotState.isDisabled()) {
cancel();
}
if (isCanceled()) {
return false;
}
if (!m_initialized) {
m_initialized = true;
startTiming();
_initialize();
initialize();
}
_execute();
execute();
return !isFinished();
}
/**
* The initialize method is called the first time this Command is run after being started.
*/
protected void initialize() {}
/**
* A shadow method called before {@link Command#initialize() initialize()}.
*/
@SuppressWarnings("MethodName")
void _initialize() {
}
/**
* The execute method is called repeatedly until this Command either finishes or is canceled.
*/
@SuppressWarnings("MethodName")
protected void execute() {}
/**
* A shadow method called before {@link Command#execute() execute()}.
*/
@SuppressWarnings("MethodName")
void _execute() {
}
/**
* Returns whether this command is finished. If it is, then the command will be removed and {@link
* Command#end() end()} will be called.
*
* <p>It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()}
* method for time-sensitive commands.
*
* <p>Returning false will result in the command never ending automatically. It may still be
* cancelled manually or interrupted by another command. Returning true will result in the
* command executing once and finishing immediately. We recommend using {@link InstantCommand}
* for this.
*
* @return whether this command is finished.
* @see Command#isTimedOut() isTimedOut()
*/
protected abstract boolean isFinished();
/**
* Called when the command ended peacefully. This is where you may want to wrap up loose ends,
* like shutting off a motor that was being used in the command.
*/
protected void end() {}
/**
* A shadow method called after {@link Command#end() end()}.
*/
@SuppressWarnings("MethodName")
void _end() {
}
/**
* Called when the command ends because somebody called {@link Command#cancel() cancel()} or
* another command shared the same requirements as this one, and booted it out.
*
* <p>This is where you may want to wrap up loose ends, like shutting off a motor that was being
* used in the command.
*
* <p>Generally, it is useful to simply call the {@link Command#end() end()} method within this
* method, as done here.
*/
protected void interrupted() {
end();
}
/**
* A shadow method called after {@link Command#interrupted() interrupted()}.
*/
@SuppressWarnings("MethodName")
void _interrupted() {}
/**
* Called to indicate that the timer should start. This is called right before {@link
* Command#initialize() initialize()} is, inside the {@link Command#run() run()} method.
*/
private void startTiming() {
m_startTime = Timer.getFPGATimestamp();
}
/**
* Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()} method
* returns a number which is greater than or equal to the timeout for the command. If there is no
* timeout, this will always return false.
*
* @return whether the time has expired
*/
protected synchronized boolean isTimedOut() {
return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
}
/**
* Returns the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
* Subsystems}) of this command.
*
* @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
* Subsystems}) of this command
*/
synchronized Enumeration getRequirements() {
return m_requirements.getElements();
}
/**
* Prevents further changes from being made.
*/
synchronized void lockChanges() {
m_locked = true;
}
/**
* If changes are locked, then this will throw an {@link IllegalUseOfCommandException}.
*
* @param message the message to say (it is appended by a default message)
*/
synchronized void validate(String message) {
if (m_locked) {
throw new IllegalUseOfCommandException(message
+ " after being started or being added to a command group");
}
}
/**
* Sets the parent of this command. No actual change is made to the group.
*
* @param parent the parent
* @throws IllegalUseOfCommandException if this {@link Command} already is already in a group
*/
synchronized void setParent(CommandGroup parent) {
if (m_parent != null) {
throw new IllegalUseOfCommandException(
"Can not give command to a command group after already being put in a command group");
}
lockChanges();
m_parent = parent;
}
/**
* Returns whether the command has a parent.
*
* @return true if the command has a parent.
*/
synchronized boolean isParented() {
return m_parent != null;
}
/**
* Clears list of subsystem requirements. This is only used by
* {@link ConditionalCommand} so cancelling the chosen command works properly
* in {@link CommandGroup}.
*/
protected void clearRequirements() {
m_requirements.clear();
}
/**
* Starts up the command. Gets the command ready to start. <p> Note that the command will
* eventually start, however it will not necessarily do so immediately, and may in fact be
* canceled before initialize is even called. </p>
*
* @throws IllegalUseOfCommandException if the command is a part of a CommandGroup
*/
public synchronized void start() {
lockChanges();
if (m_parent != null) {
throw new IllegalUseOfCommandException(
"Can not start a command that is a part of a command group");
}
Scheduler.getInstance().add(this);
m_completed = false;
}
/**
* This is used internally to mark that the command has been started. The lifecycle of a command
* is:
*
* <p>startRunning() is called. run() is called (multiple times potentially) removed() is called.
*
* <p>It is very important that startRunning and removed be called in order or some assumptions of
* the code will be broken.
*/
synchronized void startRunning() {
m_running = true;
m_startTime = -1;
}
/**
* Returns whether or not the command is running. This may return true even if the command has
* just been canceled, as it may not have yet called {@link Command#interrupted()}.
*
* @return whether or not the command is running
*/
public synchronized boolean isRunning() {
return m_running;
}
/**
* This will cancel the current command. <p> This will cancel the current command eventually. It
* can be called multiple times. And it can be called when the command is not running. If the
* command is running though, then the command will be marked as canceled and eventually removed.
* </p> <p> A command can not be canceled if it is a part of a command group, you must cancel the
* command group instead. </p>
*
* @throws IllegalUseOfCommandException if this command is a part of a command group
*/
public synchronized void cancel() {
if (m_parent != null) {
throw new IllegalUseOfCommandException("Can not manually cancel a command in a command "
+ "group");
}
_cancel();
}
/**
* This works like cancel(), except that it doesn't throw an exception if it is a part of a
* command group. Should only be called by the parent command group.
*/
@SuppressWarnings("MethodName")
synchronized void _cancel() {
if (isRunning()) {
m_canceled = true;
}
}
/**
* Returns whether or not this has been canceled.
*
* @return whether or not this has been canceled
*/
public synchronized boolean isCanceled() {
return m_canceled;
}
/**
* Whether or not this command has completed running.
*
* @return whether or not this command has completed running.
*/
public synchronized boolean isCompleted() {
return m_completed;
}
/**
* Returns whether or not this command can be interrupted.
*
* @return whether or not this command can be interrupted
*/
public synchronized boolean isInterruptible() {
return m_interruptible;
}
/**
* Sets whether or not this command can be interrupted.
*
* @param interruptible whether or not this command can be interrupted
*/
protected synchronized void setInterruptible(boolean interruptible) {
m_interruptible = interruptible;
}
/**
* Checks if the command requires the given {@link Subsystem}.
*
* @param system the system
* @return whether or not the subsystem is required, or false if given null
*/
public synchronized boolean doesRequire(Subsystem system) {
return m_requirements.contains(system);
}
/**
* Returns the {@link CommandGroup} that this command is a part of. Will return null if this
* {@link Command} is not in a group.
*
* @return the {@link CommandGroup} that this command is a part of (or null if not in group)
*/
public synchronized CommandGroup getGroup() {
return m_parent;
}
/**
* Sets whether or not this {@link Command} should run when the robot is disabled.
*
* <p>By default a command will not run when the robot is disabled, and will in fact be canceled.
*
* @param run whether or not this command should run when the robot is disabled
*/
public void setRunWhenDisabled(boolean run) {
m_runWhenDisabled = run;
}
/**
* Returns whether or not this {@link Command} will run when the robot is disabled, or if it will
* cancel itself.
*
* @return True if this command will run when the robot is disabled.
*/
public boolean willRunWhenDisabled() {
return m_runWhenDisabled;
}
/**
* Gets the name of this Command.
*
* @return Name
*/
@Override
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Command.
*
* @param name name
*/
@Override
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Command.
*
* @return Subsystem name
*/
@Override
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Command.
*
* @param subsystem subsystem name
*/
@Override
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* The string representation for a {@link Command} is by default its name.
*
* @return the string representation of this object
*/
@Override
public String toString() {
return getName();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Command");
builder.addStringProperty(".name", this::getName, null);
builder.addBooleanProperty("running", this::isRunning, value -> {
if (value) {
if (!isRunning()) {
start();
}
} else {
if (isRunning()) {
cancel();
}
}
});
builder.addBooleanProperty(".isParented", this::isParented, null);
}
}

422
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
View File

@@ -1,422 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
import java.util.Vector;
import static java.util.Objects.requireNonNull;
/**
* A {@link CommandGroup} is a list of commands which are executed in sequence.
*
* <p> Commands in a {@link CommandGroup} are added using the {@link
* CommandGroup#addSequential(Command) addSequential(...)} method and are called sequentially.
* {@link CommandGroup CommandGroups} are themselves {@link Command commands} and can be given to
* other {@link CommandGroup CommandGroups}. </p>
*
* <p> {@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command
* subcommands}. Additional requirements can be specified by calling {@link
* CommandGroup#requires(Subsystem) requires(...)} normally in the constructor. </P>
*
* <p> CommandGroups can also execute commands in parallel, simply by adding them using {@link
* CommandGroup#addParallel(Command) addParallel(...)}. </p>
*
* @see Command
* @see Subsystem
* @see IllegalUseOfCommandException
*/
@SuppressWarnings("PMD.TooManyMethods")
public class CommandGroup extends Command {
/**
* The commands in this group (stored in entries).
*/
@SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
private final Vector<Entry> m_commands = new Vector<>();
/*
* Intentionally package private
*/
/**
* The active children in this group (stored in entries).
*/
@SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
final Vector<Entry> m_children = new Vector<>();
/**
* The current command, -1 signifies that none have been run.
*/
private int m_currentCommandIndex = -1;
/**
* Creates a new {@link CommandGroup CommandGroup}. The name of this command will be set to its
* class name.
*/
public CommandGroup() {
}
/**
* Creates a new {@link CommandGroup CommandGroup} with the given name.
*
* @param name the name for this command group
* @throws IllegalArgumentException if name is null
*/
public CommandGroup(String name) {
super(name);
}
/**
* Adds a new {@link Command Command} to the group. The {@link Command Command} will be started
* after all the previously added {@link Command Commands}.
*
* <p> Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after being
* added to a group. </p>
*
* <p> It is recommended that this method be called in the constructor. </p>
*
* @param command The {@link Command Command} to be added
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another group
* @throws IllegalArgumentException if command is null
*/
public final synchronized void addSequential(Command command) {
validate("Can not add new command to command group");
if (command == null) {
throw new IllegalArgumentException("Given null command");
}
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
/**
* Adds a new {@link Command Command} to the group with a given timeout. The {@link Command
* Command} will be started after all the previously added commands.
*
* <p> Once the {@link Command Command} is started, it will be run until it finishes or the time
* expires, whichever is sooner. Note that the given {@link Command Command} will have no
* knowledge that it is on a timer. </p>
*
* <p> Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after being
* added to a group. </p>
*
* <p> It is recommended that this method be called in the constructor. </p>
*
* @param command The {@link Command Command} to be added
* @param timeout The timeout (in seconds)
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another group or if the {@link Command Command} has been
* started before or been given to another group
* @throws IllegalArgumentException if command is null or timeout is negative
*/
public final synchronized void addSequential(Command command, double timeout) {
validate("Can not add new command to command group");
if (command == null) {
throw new IllegalArgumentException("Given null command");
}
if (timeout < 0) {
throw new IllegalArgumentException("Can not be given a negative timeout");
}
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
/**
* Adds a new child {@link Command} to the group. The {@link Command} will be started after all
* the previously added {@link Command Commands}.
*
* <p> Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
* same time as the subsequent {@link Command Commands}. The child will run until either it
* finishes, a new child with conflicting requirements is started, or the main sequence runs a
* {@link Command} with conflicting requirements. In the latter two cases, the child will be
* canceled even if it says it can't be interrupted. </p>
*
* <p> Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after being
* added to a group. </p>
*
* <p> It is recommended that this method be called in the constructor. </p>
*
* @param command The command to be added
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another command group
* @throws IllegalArgumentException if command is null
*/
public final synchronized void addParallel(Command command) {
requireNonNull(command, "Provided command was null");
validate("Can not add new command to command group");
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
/**
* Adds a new child {@link Command} to the group with the given timeout. The {@link Command} will
* be started after all the previously added {@link Command Commands}.
*
* <p> Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
* or the time expires, whichever is sooner. Note that the given {@link Command Command} will have
* no knowledge that it is on a timer. </p>
*
* <p> Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
* same time as the subsequent {@link Command Commands}. The child will run until either it
* finishes, the timeout expires, a new child with conflicting requirements is started, or the
* main sequence runs a {@link Command} with conflicting requirements. In the latter two cases,
* the child will be canceled even if it says it can't be interrupted. </p>
*
* <p> Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after being
* added to a group. </p>
*
* <p> It is recommended that this method be called in the constructor. </p>
*
* @param command The command to be added
* @param timeout The timeout (in seconds)
* @throws IllegalUseOfCommandException if the group has been started before or been given to
* another command group
* @throws IllegalArgumentException if command is null
*/
public final synchronized void addParallel(Command command, double timeout) {
requireNonNull(command, "Provided command was null");
if (timeout < 0) {
throw new IllegalArgumentException("Can not be given a negative timeout");
}
validate("Can not add new command to command group");
command.setParent(this);
m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
@Override
@SuppressWarnings("MethodName")
void _initialize() {
m_currentCommandIndex = -1;
}
@Override
@SuppressWarnings({"MethodName", "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
void _execute() {
Entry entry = null;
Command cmd = null;
boolean firstRun = false;
if (m_currentCommandIndex == -1) {
firstRun = true;
m_currentCommandIndex = 0;
}
while (m_currentCommandIndex < m_commands.size()) {
if (cmd != null) {
if (entry.isTimedOut()) {
cmd._cancel();
}
if (cmd.run()) {
break;
} else {
cmd.removed();
m_currentCommandIndex++;
firstRun = true;
cmd = null;
continue;
}
}
entry = m_commands.elementAt(m_currentCommandIndex);
cmd = null;
switch (entry.m_state) {
case Entry.IN_SEQUENCE:
cmd = entry.m_command;
if (firstRun) {
cmd.startRunning();
cancelConflicts(cmd);
}
firstRun = false;
break;
case Entry.BRANCH_PEER:
m_currentCommandIndex++;
entry.m_command.start();
break;
case Entry.BRANCH_CHILD:
m_currentCommandIndex++;
cancelConflicts(entry.m_command);
entry.m_command.startRunning();
m_children.addElement(entry);
break;
default:
break;
}
}
// Run Children
for (int i = 0; i < m_children.size(); i++) {
entry = m_children.elementAt(i);
Command child = entry.m_command;
if (entry.isTimedOut()) {
child._cancel();
}
if (!child.run()) {
child.removed();
m_children.removeElementAt(i--);
}
}
}
@Override
@SuppressWarnings("MethodName")
void _end() {
// Theoretically, we don't have to check this, but we do if teams override
// the isFinished method
if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
cmd._cancel();
cmd.removed();
}
Enumeration children = m_children.elements();
while (children.hasMoreElements()) {
Command cmd = ((Entry) children.nextElement()).m_command;
cmd._cancel();
cmd.removed();
}
m_children.removeAllElements();
}
@Override
@SuppressWarnings("MethodName")
void _interrupted() {
_end();
}
/**
* Returns true if all the {@link Command Commands} in this group have been started and have
* finished.
*
* <p> Teams may override this method, although they should probably reference super.isFinished()
* if they do. </p>
*
* @return whether this {@link CommandGroup} is finished
*/
@Override
protected boolean isFinished() {
return m_currentCommandIndex >= m_commands.size() && m_children.isEmpty();
}
// Can be overwritten by teams
@Override
protected void initialize() {
}
// Can be overwritten by teams
@Override
protected void execute() {
}
// Can be overwritten by teams
@Override
protected void end() {
}
// Can be overwritten by teams
@Override
protected void interrupted() {
}
/**
* Returns whether or not this group is interruptible. A command group will be uninterruptible if
* {@link CommandGroup#setInterruptible(boolean) setInterruptable(false)} was called or if it is
* currently running an uninterruptible command or child.
*
* @return whether or not this {@link CommandGroup} is interruptible.
*/
@Override
public synchronized boolean isInterruptible() {
if (!super.isInterruptible()) {
return false;
}
if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
Command cmd = m_commands.elementAt(m_currentCommandIndex).m_command;
if (!cmd.isInterruptible()) {
return false;
}
}
for (int i = 0; i < m_children.size(); i++) {
if (!m_children.elementAt(i).m_command.isInterruptible()) {
return false;
}
}
return true;
}
private void cancelConflicts(Command command) {
for (int i = 0; i < m_children.size(); i++) {
Command child = m_children.elementAt(i).m_command;
Enumeration requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
Object requirement = requirements.nextElement();
if (child.doesRequire((Subsystem) requirement)) {
child._cancel();
child.removed();
m_children.removeElementAt(i--);
break;
}
}
}
}
private static class Entry {
private static final int IN_SEQUENCE = 0;
private static final int BRANCH_PEER = 1;
private static final int BRANCH_CHILD = 2;
private final Command m_command;
private final int m_state;
private final double m_timeout;
Entry(Command command, int state) {
m_command = command;
m_state = state;
m_timeout = -1;
}
Entry(Command command, int state, double timeout) {
m_command = command;
m_state = state;
m_timeout = timeout;
}
boolean isTimedOut() {
if (m_timeout == -1) {
return false;
} else {
double time = m_command.timeSinceInitialized();
return time != 0 && time >= m_timeout;
}
}
}
}

180
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
View File

@@ -1,180 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
/**
* A {@link ConditionalCommand} is a {@link Command} that starts one of two commands.
*
* <p>
* A {@link ConditionalCommand} uses m_condition to determine whether it should run m_onTrue or
* m_onFalse.
* </p>
*
* <p>
* A {@link ConditionalCommand} adds the proper {@link Command} to the {@link Scheduler} during
* {@link ConditionalCommand#initialize()} and then {@link ConditionalCommand#isFinished()} will
* return true once that {@link Command} has finished executing.
* </p>
*
* <p>
* If no {@link Command} is specified for m_onFalse, the occurrence of that condition will be a
* no-op.
* </p>
*
* <p>
* A ConditionalCommand will require the superset of subsystems of the onTrue
* and onFalse commands.
* </p>
*
* @see Command
* @see Scheduler
*/
public abstract class ConditionalCommand extends Command {
/**
* The Command to execute if {@link ConditionalCommand#condition()} returns true.
*/
private Command m_onTrue;
/**
* The Command to execute if {@link ConditionalCommand#condition()} returns false.
*/
private Command m_onFalse;
/**
* Stores command chosen by condition.
*/
private Command m_chosenCommand;
private void requireAll() {
if (m_onTrue != null) {
for (Enumeration e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
if (m_onFalse != null) {
for (Enumeration e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
requires((Subsystem) e.nextElement());
}
}
}
/**
* Creates a new ConditionalCommand with given onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
*/
public ConditionalCommand(Command onTrue) {
this(onTrue, null);
}
/**
* Creates a new ConditionalCommand with given onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
* @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false
*/
public ConditionalCommand(Command onTrue, Command onFalse) {
m_onTrue = onTrue;
m_onFalse = onFalse;
requireAll();
}
/**
* Creates a new ConditionalCommand with given name and onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param name the name for this command group
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
*/
public ConditionalCommand(String name, Command onTrue) {
this(name, onTrue, null);
}
/**
* Creates a new ConditionalCommand with given name and onTrue and onFalse Commands.
*
* <p>Users of this constructor should also override condition().
*
* @param name the name for this command group
* @param onTrue The Command to execute if {@link ConditionalCommand#condition()} returns true
* @param onFalse The Command to execute if {@link ConditionalCommand#condition()} returns false
*/
public ConditionalCommand(String name, Command onTrue, Command onFalse) {
super(name);
m_onTrue = onTrue;
m_onFalse = onFalse;
requireAll();
}
/**
* The Condition to test to determine which Command to run.
*
* @return true if m_onTrue should be run or false if m_onFalse should be run.
*/
protected abstract boolean condition();
/**
* Calls {@link ConditionalCommand#condition()} and runs the proper command.
*/
@Override
protected void _initialize() {
if (condition()) {
m_chosenCommand = m_onTrue;
} else {
m_chosenCommand = m_onFalse;
}
if (m_chosenCommand != null) {
/*
* This is a hack to make cancelling the chosen command inside a
* CommandGroup work properly
*/
m_chosenCommand.clearRequirements();
m_chosenCommand.start();
}
super._initialize();
}
@Override
protected synchronized void _cancel() {
if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
m_chosenCommand.cancel();
}
super._cancel();
}
@Override
protected boolean isFinished() {
if (m_chosenCommand != null) {
return m_chosenCommand.isCompleted();
} else {
return true;
}
}
@Override
protected void _interrupted() {
if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
m_chosenCommand.cancel();
}
super._interrupted();
}
}

36
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
View File

@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* This exception will be thrown if a command is used illegally. There are several ways for this to
* happen.
*
* <p> Basically, a command becomes "locked" after it is first started or added to a command group.
* </p>
*
* <p> This exception should be thrown if (after a command has been locked) its requirements change,
* it is put into multiple command groups, it is started from outside its command group, or it adds
* a new child. </p>
*/
public class IllegalUseOfCommandException extends RuntimeException {
/**
* Instantiates an {@link IllegalUseOfCommandException}.
*/
public IllegalUseOfCommandException() {
}
/**
* Instantiates an {@link IllegalUseOfCommandException} with the given message.
*
* @param message the message
*/
public IllegalUseOfCommandException(String message) {
super(message);
}
}

110
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
View File

@@ -1,110 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* This command will execute once, then finish immediately afterward.
*
* <p>Subclassing {@link InstantCommand} is shorthand for returning true from
* {@link Command isFinished}.
*/
public class InstantCommand extends Command {
private Runnable m_func;
public InstantCommand() {
}
/**
* Creates a new {@link InstantCommand InstantCommand} with the given name.
*
* @param name the name for this command
*/
public InstantCommand(String name) {
super(name);
}
/**
* Creates a new {@link InstantCommand InstantCommand} with the given requirement.
*
* @param subsystem the subsystem this command requires
*/
public InstantCommand(Subsystem subsystem) {
super(subsystem);
}
/**
* Creates a new {@link InstantCommand InstantCommand} with the given name and requirement.
*
* @param name the name for this command
* @param subsystem the subsystem this command requires
*/
public InstantCommand(String name, Subsystem subsystem) {
super(name, subsystem);
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(Runnable func) {
m_func = func;
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param name the name for this command
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(String name, Runnable func) {
super(name);
m_func = func;
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param requirement the subsystem this command requires
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(Subsystem requirement, Runnable func) {
super(requirement);
m_func = func;
}
/**
* Creates a new {@link InstantCommand InstantCommand}.
*
* @param name the name for this command
* @param requirement the subsystem this command requires
* @param func the function to run when {@link Command#initialize() initialize()} is run
*/
public InstantCommand(String name, Subsystem requirement, Runnable func) {
super(name, requirement);
m_func = func;
}
@Override
protected boolean isFinished() {
return true;
}
/**
* Trigger the stored function.
*
* <p>Called just before this Command runs the first time.
*/
@Override
protected void _initialize() {
super._initialize();
if (m_func != null) {
m_func.run();
}
}
}

63
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
View File

@@ -1,63 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* An element that is in a LinkedList.
*/
class LinkedListElement {
private LinkedListElement m_next;
private LinkedListElement m_previous;
private Command m_data;
public void setData(Command newData) {
m_data = newData;
}
public Command getData() {
return m_data;
}
public LinkedListElement getNext() {
return m_next;
}
public LinkedListElement getPrevious() {
return m_previous;
}
public void add(LinkedListElement listElement) {
if (m_next == null) {
m_next = listElement;
m_next.m_previous = this;
} else {
m_next.m_previous = listElement;
listElement.m_next = m_next;
listElement.m_previous = this;
m_next = listElement;
}
}
@SuppressWarnings("PMD.EmptyIfStmt")
public LinkedListElement remove() {
if (m_previous == null && m_next == null) {
// no-op
} else if (m_next == null) {
m_previous.m_next = null;
} else if (m_previous == null) {
m_next.m_previous = null;
} else {
m_next.m_previous = m_previous;
m_previous.m_next = m_next;
}
LinkedListElement returnNext = m_next;
m_next = null;
m_previous = null;
return returnNext;
}
}

284
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
View File

@@ -1,284 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
/**
* This class defines a {@link Command} which interacts heavily with a PID loop.
*
* <p> It provides some convenience methods to run an internal {@link PIDController} . It will also
* start and stop said {@link PIDController} when the {@link PIDCommand} is first initialized and
* ended/interrupted. </p>
*/
public abstract class PIDCommand extends Command {
/**
* The internal {@link PIDController}.
*/
private final PIDController m_controller;
/**
* An output which calls {@link PIDCommand#usePIDOutput(double)}.
*/
private final PIDOutput m_output = this::usePIDOutput;
/**
* A source which calls {@link PIDCommand#returnPIDInput()}.
*/
private final PIDSource m_source = new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
@Override
public double pidGet() {
return returnPIDInput();
}
};
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values.
*
* @param name the name of the command
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
@SuppressWarnings("ParameterName")
public PIDCommand(String name, double p, double i, double d) {
super(name);
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
* the time between PID loop calculations to be equal to the given period.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
*/
@SuppressWarnings("ParameterName")
public PIDCommand(String name, double p, double i, double d, double period) {
super(name);
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
@SuppressWarnings("ParameterName")
public PIDCommand(double p, double i, double d) {
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
*/
@SuppressWarnings("ParameterName")
public PIDCommand(double p, double i, double d, double period) {
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values.
*
* @param name the name of the command
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param subsystem the subsystem that this command requires
*/
@SuppressWarnings("ParameterName")
public PIDCommand(String name, double p, double i, double d, Subsystem subsystem) {
super(name, subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
* the time between PID loop calculations to be equal to the given period.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
* @param subsystem the subsystem that this command requires
*/
@SuppressWarnings("ParameterName")
public PIDCommand(String name, double p, double i, double d, double period,
Subsystem subsystem) {
super(name, subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param subsystem the subsystem that this command requires
*/
@SuppressWarnings("ParameterName")
public PIDCommand(double p, double i, double d, Subsystem subsystem) {
super(subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output);
}
/**
* Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
* @param subsystem the subsystem that this command requires
*/
@SuppressWarnings("ParameterName")
public PIDCommand(double p, double i, double d, double period, Subsystem subsystem) {
super(subsystem);
m_controller = new PIDController(p, i, d, m_source, m_output, period);
}
/**
* Returns the {@link PIDController} used by this {@link PIDCommand}. Use this if you would like
* to fine tune the pid loop.
*
* @return the {@link PIDController} used by this {@link PIDCommand}
*/
protected PIDController getPIDController() {
return m_controller;
}
@Override
@SuppressWarnings("MethodName")
void _initialize() {
m_controller.enable();
}
@Override
@SuppressWarnings("MethodName")
void _end() {
m_controller.disable();
}
@Override
@SuppressWarnings("MethodName")
void _interrupted() {
_end();
}
/**
* Adds the given value to the setpoint. If {@link PIDCommand#setInputRange(double, double)
* setInputRange(...)} was used, then the bounds will still be honored by this method.
*
* @param deltaSetpoint the change in the setpoint
*/
public void setSetpointRelative(double deltaSetpoint) {
setSetpoint(getSetpoint() + deltaSetpoint);
}
/**
* Sets the setpoint to the given value. If {@link PIDCommand#setInputRange(double, double)
* setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
* range.
*
* @param setpoint the new setpoint
*/
protected void setSetpoint(double setpoint) {
m_controller.setSetpoint(setpoint);
}
/**
* Returns the setpoint.
*
* @return the setpoint
*/
protected double getSetpoint() {
return m_controller.getSetpoint();
}
/**
* Returns the current position.
*
* @return the current position
*/
protected double getPosition() {
return returnPIDInput();
}
/**
* Sets the maximum and minimum values expected from the input and setpoint.
*
* @param minimumInput the minimum value expected from the input and setpoint
* @param maximumInput the maximum value expected from the input and setpoint
*/
protected void setInputRange(double minimumInput, double maximumInput) {
m_controller.setInputRange(minimumInput, maximumInput);
}
/**
* Returns the input for the pid loop.
*
* <p>It returns the input for the pid loop, so if this command was based off of a gyro, then it
* should return the angle of the gyro.
*
* <p>All subclasses of {@link PIDCommand} must override this method.
*
* <p>This method will be called in a different thread then the {@link Scheduler} thread.
*
* @return the value the pid loop should use as input
*/
protected abstract double returnPIDInput();
/**
* Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
* This method is a good time to set motor values, maybe something along the lines of
* <code>driveline.tankDrive(output, -output)</code>
*
* <p>All subclasses of {@link PIDCommand} must override this method.
*
* <p>This method will be called in a different thread then the {@link Scheduler} thread.
*
* @param output the value the pid loop calculated
*/
protected abstract void usePIDOutput(double output);
@Override
public void initSendable(SendableBuilder builder) {
m_controller.initSendable(builder);
super.initSendable(builder);
builder.setSmartDashboardType("PIDCommand");
}
}

287
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
View File

@@ -1,287 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* This class is designed to handle the case where there is a {@link Subsystem} which uses a single
* {@link PIDController} almost constantly (for instance, an elevator which attempts to stay at a
* constant height).
*
* <p>It provides some convenience methods to run an internal {@link PIDController} . It also
* allows access to the internal {@link PIDController} in order to give total control to the
* programmer.
*/
public abstract class PIDSubsystem extends Subsystem {
/**
* The internal {@link PIDController}.
*/
private final PIDController m_controller;
/**
* An output which calls {@link PIDCommand#usePIDOutput(double)}.
*/
private final PIDOutput m_output = this::usePIDOutput;
/**
* A source which calls {@link PIDCommand#returnPIDInput()}.
*/
private final PIDSource m_source = new PIDSource() {
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
}
@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
@Override
public double pidGet() {
return returnPIDInput();
}
};
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
@SuppressWarnings("ParameterName")
public PIDSubsystem(String name, double p, double i, double d) {
super(name);
m_controller = new PIDController(p, i, d, m_source, m_output);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feed forward value
*/
@SuppressWarnings("ParameterName")
public PIDSubsystem(String name, double p, double i, double d, double f) {
super(name);
m_controller = new PIDController(p, i, d, f, m_source, m_output);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will also
* space the time between PID loop calculations to be equal to the given period.
*
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feed forward value
* @param period the time (in seconds) between calculations
*/
@SuppressWarnings("ParameterName")
public PIDSubsystem(String name, double p, double i, double d, double f, double period) {
super(name);
m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
* class name as its name.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
@SuppressWarnings("ParameterName")
public PIDSubsystem(double p, double i, double d) {
m_controller = new PIDController(p, i, d, m_source, m_output);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feed forward coefficient
* @param period the time (in seconds) between calculations
*/
@SuppressWarnings("ParameterName")
public PIDSubsystem(double p, double i, double d, double f, double period) {
m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
addChild("PIDController", m_controller);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
* class name as its name. It will also space the time between PID loop calculations to be equal
* to the given period.
*
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param period the time (in seconds) between calculations
*/
@SuppressWarnings("ParameterName")
public PIDSubsystem(double p, double i, double d, double period) {
m_controller = new PIDController(p, i, d, m_source, m_output, period);
addChild("PIDController", m_controller);
}
/**
* Returns the {@link PIDController} used by this {@link PIDSubsystem}. Use this if you would like
* to fine tune the pid loop.
*
* @return the {@link PIDController} used by this {@link PIDSubsystem}
*/
public PIDController getPIDController() {
return m_controller;
}
/**
* Adds the given value to the setpoint. If {@link PIDSubsystem#setInputRange(double, double)
* setInputRange(...)} was used, then the bounds will still be honored by this method.
*
* @param deltaSetpoint the change in the setpoint
*/
public void setSetpointRelative(double deltaSetpoint) {
setSetpoint(getPosition() + deltaSetpoint);
}
/**
* Sets the setpoint to the given value. If {@link PIDSubsystem#setInputRange(double, double)
* setInputRange(...)} was called, then the given setpoint will be trimmed to fit within the
* range.
*
* @param setpoint the new setpoint
*/
public void setSetpoint(double setpoint) {
m_controller.setSetpoint(setpoint);
}
/**
* Returns the setpoint.
*
* @return the setpoint
*/
public double getSetpoint() {
return m_controller.getSetpoint();
}
/**
* Returns the current position.
*
* @return the current position
*/
public double getPosition() {
return returnPIDInput();
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
public void setInputRange(double minimumInput, double maximumInput) {
m_controller.setInputRange(minimumInput, maximumInput);
}
/**
* Sets the maximum and minimum values to write.
*
* @param minimumOutput the minimum value to write to the output
* @param maximumOutput the maximum value to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
m_controller.setOutputRange(minimumOutput, maximumOutput);
}
/**
* Set the absolute error which is considered tolerable for use with OnTarget. The value is in the
* same range as the PIDInput values.
*
* @param t the absolute tolerance
*/
@SuppressWarnings("ParameterName")
public void setAbsoluteTolerance(double t) {
m_controller.setAbsoluteTolerance(t);
}
/**
* Set the percentage error which is considered tolerable for use with OnTarget. (Value of 15.0 ==
* 15 percent).
*
* @param p the percent tolerance
*/
@SuppressWarnings("ParameterName")
public void setPercentTolerance(double p) {
m_controller.setPercentTolerance(p);
}
/**
* Return true if the error is within the percentage of the total input range, determined by
* setTolerance. This assumes that the maximum and minimum input were set using setInput.
*
* @return true if the error is less than the tolerance
*/
public boolean onTarget() {
return m_controller.onTarget();
}
/**
* Returns the input for the pid loop.
*
* <p>It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then
* it should return the angle of the gyro.
*
* <p>All subclasses of {@link PIDSubsystem} must override this method.
*
* @return the value the pid loop should use as input
*/
protected abstract double returnPIDInput();
/**
* Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
* This method is a good time to set motor values, maybe something along the lines of
* <code>driveline.tankDrive(output, -output)</code>.
*
* <p>All subclasses of {@link PIDSubsystem} must override this method.
*
* @param output the value the pid loop calculated
*/
protected abstract void usePIDOutput(double output);
/**
* Enables the internal {@link PIDController}.
*/
public void enable() {
m_controller.enable();
}
/**
* Disables the internal {@link PIDController}.
*/
public void disable() {
m_controller.disable();
}
}

35
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
View File

@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* A {@link PrintCommand} is a command which prints out a string when it is initialized, and then
* immediately finishes. It is useful if you want a {@link CommandGroup} to print out a string when
* it reaches a certain point.
*/
public class PrintCommand extends InstantCommand {
/**
* The message to print out.
*/
private final String m_message;
/**
* Instantiates a {@link PrintCommand} which will print the given message when it is run.
*
* @param message the message to print
*/
public PrintCommand(String message) {
super("Print(\"" + message + "\"");
m_message = message;
}
@Override
protected void initialize() {
System.out.println(m_message);
}
}

360
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
View File

@@ -1,360 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
import java.util.Hashtable;
import java.util.Vector;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* The {@link Scheduler} is a singleton which holds the top-level running commands. It is in charge
* of both calling the command's {@link Command#run() run()} method and to make sure that there are
* no two commands with conflicting requirements running.
*
* <p> It is fine if teams wish to take control of the {@link Scheduler} themselves, all that needs
* to be done is to call {@link Scheduler#getInstance() Scheduler.getInstance()}.{@link
* Scheduler#run() run()} often to have {@link Command Commands} function correctly. However, this
* is already done for you if you use the CommandBased Robot template. </p>
*
* @see Command
*/
@SuppressWarnings("PMD.TooManyMethods")
public final class Scheduler implements Sendable, AutoCloseable {
/**
* The Singleton Instance.
*/
private static Scheduler instance;
/**
* Returns the {@link Scheduler}, creating it if one does not exist.
*
* @return the {@link Scheduler}
*/
public static synchronized Scheduler getInstance() {
if (instance == null) {
instance = new Scheduler();
}
return instance;
}
/**
* A hashtable of active {@link Command Commands} to their {@link LinkedListElement}.
*/
@SuppressWarnings("PMD.LooseCoupling")
private final Hashtable<Command, LinkedListElement> m_commandTable = new Hashtable<>();
/**
* The {@link Set} of all {@link Subsystem Subsystems}.
*/
private final Set m_subsystems = new Set();
/**
* The first {@link Command} in the list.
*/
private LinkedListElement m_firstCommand;
/**
* The last {@link Command} in the list.
*/
private LinkedListElement m_lastCommand;
/**
* Whether or not we are currently adding a command.
*/
private boolean m_adding;
/**
* Whether or not we are currently disabled.
*/
private boolean m_disabled;
/**
* A list of all {@link Command Commands} which need to be added.
*/
@SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
private final Vector<Command> m_additions = new Vector<>();
private NetworkTableEntry m_namesEntry;
private NetworkTableEntry m_idsEntry;
private NetworkTableEntry m_cancelEntry;
/**
* A list of all {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It is
* created lazily.
*/
@SuppressWarnings("PMD.LooseCoupling")
private Vector<ButtonScheduler> m_buttons;
private boolean m_runningCommandsChanged;
/**
* Instantiates a {@link Scheduler}.
*/
private Scheduler() {
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
SendableRegistry.addLW(this, "Scheduler");
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Adds the command to the {@link Scheduler}. This will not add the {@link Command} immediately,
* but will instead wait for the proper time in the {@link Scheduler#run()} loop before doing so.
* The command returns immediately and does nothing if given null.
*
* <p> Adding a {@link Command} to the {@link Scheduler} involves the {@link Scheduler} removing
* any {@link Command} which has shared requirements. </p>
*
* @param command the command to add
*/
public void add(Command command) {
if (command != null) {
m_additions.addElement(command);
}
}
/**
* Adds a button to the {@link Scheduler}. The {@link Scheduler} will poll the button during its
* {@link Scheduler#run()}.
*
* @param button the button to add
*/
@SuppressWarnings("PMD.UseArrayListInsteadOfVector")
public void addButton(ButtonScheduler button) {
if (m_buttons == null) {
m_buttons = new Vector<>();
}
m_buttons.addElement(button);
}
/**
* Adds a command immediately to the {@link Scheduler}. This should only be called in the {@link
* Scheduler#run()} loop. Any command with conflicting requirements will be removed, unless it is
* uninterruptable. Giving <code>null</code> does nothing.
*
* @param command the {@link Command} to add
*/
@SuppressWarnings({"MethodName", "PMD.CyclomaticComplexity"})
private void _add(Command command) {
if (command == null) {
return;
}
// Check to make sure no adding during adding
if (m_adding) {
System.err.println("WARNING: Can not start command from cancel method. Ignoring:" + command);
return;
}
// Only add if not already in
if (!m_commandTable.containsKey(command)) {
// Check that the requirements can be had
Enumeration requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
Subsystem lock = (Subsystem) requirements.nextElement();
if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
return;
}
}
// Give it the requirements
m_adding = true;
requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
Subsystem lock = (Subsystem) requirements.nextElement();
if (lock.getCurrentCommand() != null) {
lock.getCurrentCommand().cancel();
remove(lock.getCurrentCommand());
}
lock.setCurrentCommand(command);
}
m_adding = false;
// Add it to the list
LinkedListElement element = new LinkedListElement();
element.setData(command);
if (m_firstCommand == null) {
m_firstCommand = m_lastCommand = element;
} else {
m_lastCommand.add(element);
m_lastCommand = element;
}
m_commandTable.put(command, element);
m_runningCommandsChanged = true;
command.startRunning();
}
}
/**
* Runs a single iteration of the loop. This method should be called often in order to have a
* functioning {@link Command} system. The loop has five stages:
*
* <ol> <li>Poll the Buttons</li> <li>Execute/Remove the Commands</li> <li>Send values to
* SmartDashboard</li> <li>Add Commands</li> <li>Add Defaults</li> </ol>
*/
@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
public void run() {
m_runningCommandsChanged = false;
if (m_disabled) {
return;
} // Don't run when m_disabled
// Get button input (going backwards preserves button priority)
if (m_buttons != null) {
for (int i = m_buttons.size() - 1; i >= 0; i--) {
m_buttons.elementAt(i).execute();
}
}
// Call every subsystem's periodic method
Enumeration subsystems = m_subsystems.getElements();
while (subsystems.hasMoreElements()) {
((Subsystem) subsystems.nextElement()).periodic();
}
// Loop through the commands
LinkedListElement element = m_firstCommand;
while (element != null) {
Command command = element.getData();
element = element.getNext();
if (!command.run()) {
remove(command);
m_runningCommandsChanged = true;
}
}
// Add the new things
for (int i = 0; i < m_additions.size(); i++) {
_add(m_additions.elementAt(i));
}
m_additions.removeAllElements();
// Add in the defaults
Enumeration locks = m_subsystems.getElements();
while (locks.hasMoreElements()) {
Subsystem lock = (Subsystem) locks.nextElement();
if (lock.getCurrentCommand() == null) {
_add(lock.getDefaultCommand());
}
lock.confirmCommand();
}
}
/**
* Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link Scheduler} might
* know if a default {@link Command} needs to be run. All {@link Subsystem Subsystems} should call
* this.
*
* @param system the system
*/
void registerSubsystem(Subsystem system) {
if (system != null) {
m_subsystems.add(system);
}
}
/**
* Removes the {@link Command} from the {@link Scheduler}.
*
* @param command the command to remove
*/
void remove(Command command) {
if (command == null || !m_commandTable.containsKey(command)) {
return;
}
LinkedListElement element = m_commandTable.get(command);
m_commandTable.remove(command);
if (element.equals(m_lastCommand)) {
m_lastCommand = element.getPrevious();
}
if (element.equals(m_firstCommand)) {
m_firstCommand = element.getNext();
}
element.remove();
Enumeration requirements = command.getRequirements();
while (requirements.hasMoreElements()) {
((Subsystem) requirements.nextElement()).setCurrentCommand(null);
}
command.removed();
}
/**
* Removes all commands.
*/
public void removeAll() {
// TODO: Confirm that this works with "uninteruptible" commands
while (m_firstCommand != null) {
remove(m_firstCommand.getData());
}
}
/**
* Disable the command scheduler.
*/
public void disable() {
m_disabled = true;
}
/**
* Enable the command scheduler.
*/
public void enable() {
m_disabled = false;
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Scheduler");
m_namesEntry = builder.getEntry("Names");
m_idsEntry = builder.getEntry("Ids");
m_cancelEntry = builder.getEntry("Cancel");
builder.setUpdateTable(() -> {
if (m_namesEntry != null && m_idsEntry != null && m_cancelEntry != null) {
// Get the commands to cancel
double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
if (toCancel.length > 0) {
for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
for (double d : toCancel) {
if (e.getData().hashCode() == d) {
e.getData().cancel();
}
}
}
m_cancelEntry.setDoubleArray(new double[0]);
}
if (m_runningCommandsChanged) {
// Set the the running commands
int number = 0;
for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
number++;
}
String[] commands = new String[number];
double[] ids = new double[number];
number = 0;
for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
commands[number] = e.getData().getName();
ids[number] = e.getData().hashCode();
number++;
}
m_namesEntry.setStringArray(commands);
m_idsEntry.setDoubleArray(ids);
}
}
});
}
}

48
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Set.java
View File

@@ -1,48 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import java.util.Enumeration;
import java.util.Vector;
@SuppressWarnings("all")
/**
* A set.
*/
class Set {
private Vector m_set = new Vector();
public Set() {
}
public void add(Object o) {
if (m_set.contains(o)) {
return;
}
m_set.addElement(o);
}
public void add(Set s) {
Enumeration stuff = s.getElements();
for (Enumeration e = stuff; e.hasMoreElements(); ) {
add(e.nextElement());
}
}
public void clear() {
m_set.clear();
}
public boolean contains(Object o) {
return m_set.contains(o);
}
public Enumeration getElements() {
return m_set.elements();
}
}

35
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
View File

@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* A {@link StartCommand} will call the {@link Command#start() start()} method of another command
* when it is initialized and will finish immediately.
*/
public class StartCommand extends InstantCommand {
/**
* The command to fork.
*/
private final Command m_commandToFork;
/**
* Instantiates a {@link StartCommand} which will start the given command whenever its {@link
* Command#initialize() initialize()} is called.
*
* @param commandToStart the {@link Command} to start
*/
public StartCommand(Command commandToStart) {
super("Start(" + commandToStart + ")");
m_commandToFork = commandToStart;
}
@Override
protected void initialize() {
m_commandToFork.start();
}
}

255
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
View File

@@ -1,255 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import java.util.Collections;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* This class defines a major component of the robot.
*
* <p> A good example of a subsystem is the driveline, or a claw if the robot has one. </p>
*
* <p> All motors should be a part of a subsystem. For instance, all the wheel motors should be a
* part of some kind of "Driveline" subsystem. </p>
*
* <p> Subsystems are used within the command system as requirements for {@link Command}. Only one
* command which requires a subsystem can run at a time. Also, subsystems can have default commands
* which are started if there is no command running which requires this subsystem. </p>
*
* @see Command
*/
public abstract class Subsystem implements Sendable, AutoCloseable {
/**
* Whether or not getDefaultCommand() was called.
*/
private boolean m_initializedDefaultCommand;
/**
* The current command.
*/
private Command m_currentCommand;
private boolean m_currentCommandChanged;
/**
* The default command.
*/
private Command m_defaultCommand;
/**
* Creates a subsystem with the given name.
*
* @param name the name of the subsystem
*/
public Subsystem(String name) {
SendableRegistry.addLW(this, name, name);
Scheduler.getInstance().registerSubsystem(this);
}
/**
* Creates a subsystem. This will set the name to the name of the class.
*/
public Subsystem() {
String name = getClass().getName();
name = name.substring(name.lastIndexOf('.') + 1);
SendableRegistry.addLW(this, name, name);
Scheduler.getInstance().registerSubsystem(this);
m_currentCommandChanged = true;
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Initialize the default command for a subsystem By default subsystems have no default command,
* but if they do, the default command is set with this method. It is called on all Subsystems by
* CommandBase in the users program after all the Subsystems are created.
*/
protected abstract void initDefaultCommand();
/**
* When the run method of the scheduler is called this method will be called.
*/
public void periodic() {
// Override me!
}
/**
* Sets the default command. If this is not called or is called with null, then there will be no
* default command for the subsystem.
*
* <p> <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
* singleton. </p>
*
* @param command the default command (or null if there should be none)
* @throws IllegalUseOfCommandException if the command does not require the subsystem
*/
public void setDefaultCommand(Command command) {
if (command == null) {
m_defaultCommand = null;
} else {
if (!Collections.list(command.getRequirements()).contains(this)) {
throw new IllegalUseOfCommandException("A default command must require the subsystem");
}
m_defaultCommand = command;
}
}
/**
* Returns the default command (or null if there is none).
*
* @return the default command
*/
public Command getDefaultCommand() {
if (!m_initializedDefaultCommand) {
m_initializedDefaultCommand = true;
initDefaultCommand();
}
return m_defaultCommand;
}
/**
* Returns the default command name, or empty string is there is none.
*
* @return the default command name
*/
public String getDefaultCommandName() {
Command defaultCommand = getDefaultCommand();
if (defaultCommand != null) {
return defaultCommand.getName();
} else {
return "";
}
}
/**
* Sets the current command.
*
* @param command the new current command
*/
void setCurrentCommand(Command command) {
m_currentCommand = command;
m_currentCommandChanged = true;
}
/**
* Call this to alert Subsystem that the current command is actually the command. Sometimes, the
* {@link Subsystem} is told that it has no command while the {@link Scheduler} is going through
* the loop, only to be soon after given a new one. This will avoid that situation.
*/
void confirmCommand() {
if (m_currentCommandChanged) {
m_currentCommandChanged = false;
}
}
/**
* Returns the command which currently claims this subsystem.
*
* @return the command which currently claims this subsystem
*/
public Command getCurrentCommand() {
return m_currentCommand;
}
/**
* Returns the current command name, or empty string if no current command.
*
* @return the current command name
*/
public String getCurrentCommandName() {
Command currentCommand = getCurrentCommand();
if (currentCommand != null) {
return currentCommand.getName();
} else {
return "";
}
}
/**
* Associate a {@link Sendable} with this Subsystem.
* Also update the child's name.
*
* @param name name to give child
* @param child sendable
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
SendableRegistry.addChild(this, child);
}
/**
* Associate a {@link Sendable} with this Subsystem.
*
* @param child sendable
*/
public void addChild(Sendable child) {
SendableRegistry.setSubsystem(child, getSubsystem());
SendableRegistry.enableLiveWindow(child);
SendableRegistry.addChild(this, child);
}
/**
* Gets the name of this Subsystem.
*
* @return Name
*/
@Override
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Subsystem.
*
* @param name name
*/
@Override
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Subsystem.
*
* @return Subsystem name
*/
@Override
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Subsystem.
*
* @param subsystem subsystem name
*/
@Override
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
@Override
public String toString() {
return getSubsystem();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Subsystem");
builder.addBooleanProperty(".hasDefault", () -> m_defaultCommand != null, null);
builder.addStringProperty(".default", this::getDefaultCommandName, null);
builder.addBooleanProperty(".hasCommand", () -> m_currentCommand != null, null);
builder.addStringProperty(".command", this::getCurrentCommandName, null);
}
}

62
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
View File

@@ -1,62 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* A {@link TimedCommand} will wait for a timeout before finishing.
* {@link TimedCommand} is used to execute a command for a given amount of time.
*/
public class TimedCommand extends Command {
/**
* Instantiates a TimedCommand with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time the command takes to run (seconds)
*/
public TimedCommand(String name, double timeout) {
super(name, timeout);
}
/**
* Instantiates a TimedCommand with the given timeout.
*
* @param timeout the time the command takes to run (seconds)
*/
public TimedCommand(double timeout) {
super(timeout);
}
/**
* Instantiates a TimedCommand with the given name and timeout.
*
* @param name the name of the command
* @param timeout the time the command takes to run (seconds)
* @param subsystem the subsystem that this command requires
*/
public TimedCommand(String name, double timeout, Subsystem subsystem) {
super(name, timeout, subsystem);
}
/**
* Instantiates a TimedCommand with the given timeout.
*
* @param timeout the time the command takes to run (seconds)
* @param subsystem the subsystem that this command requires
*/
public TimedCommand(double timeout, Subsystem subsystem) {
super(timeout, subsystem);
}
/**
* Ends command when timed out.
*/
@Override
protected boolean isFinished() {
return isTimedOut();
}
}

35
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
View File

@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* A {@link WaitCommand} will wait for a certain amount of time before finishing. It is useful if
* you want a {@link CommandGroup} to pause for a moment.
*
* @see CommandGroup
*/
public class WaitCommand extends TimedCommand {
/**
* Instantiates a {@link WaitCommand} with the given timeout.
*
* @param timeout the time the command takes to run (seconds)
*/
public WaitCommand(double timeout) {
this("Wait(" + timeout + ")", timeout);
}
/**
* Instantiates a {@link WaitCommand} with the given timeout.
*
* @param name the name of the command
* @param timeout the time the command takes to run (seconds)
*/
public WaitCommand(String name, double timeout) {
super(name, timeout);
}
}

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
View File

@@ -1,23 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
/**
* This command will only finish if whatever {@link CommandGroup} it is in has no active children.
* If it is not a part of a {@link CommandGroup}, then it will finish immediately. If it is itself
* an active child, then the {@link CommandGroup} will never end.
*
* <p>This class is useful for the situation where you want to allow anything running in parallel
* to finish, before continuing in the main {@link CommandGroup} sequence.
*/
public class WaitForChildren extends Command {
@Override
protected boolean isFinished() {
return getGroup() == null || getGroup().m_children.isEmpty();
}
}

31
wpilibj/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
View File

@@ -1,31 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.Timer;
/**
* WaitUntilCommand - waits until an absolute game time. This will wait until the game clock reaches
* some value, then continue to the next command.
*/
public class WaitUntilCommand extends Command {
private final double m_time;
public WaitUntilCommand(double time) {
super("WaitUntil(" + time + ")");
m_time = time;
}
/**
* Check if we've reached the actual finish time.
*/
@Override
public boolean isFinished() {
return Timer.getMatchTime() >= m_time;
}
}

5
wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
View File

@@ -11,7 +11,6 @@ import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -64,17 +63,13 @@ public class LiveWindow {
startLiveWindow = enabled;
liveWindowEnabled = enabled;
updateValues(); // Force table generation now to make sure everything is defined
Scheduler scheduler = Scheduler.getInstance();
if (enabled) {
System.out.println("Starting live window mode.");
scheduler.disable();
scheduler.removeAll();
} else {
System.out.println("stopping live window mode.");
SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
cbdata.builder.stopLiveWindowMode();
});
scheduler.enable();
}
enabledEntry.setBoolean(enabled);
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
View File

@@ -16,7 +16,6 @@ import java.util.concurrent.locks.ReentrantLock;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.command.Command;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
@@ -25,7 +24,7 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
* {@link SmartDashboard}.
*
* <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
* this by putting every possible {@link Command} you want to run as an autonomous into a {@link
* this by putting every possible Command you want to run as an autonomous into a {@link
* SendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
* on the laptop. Once autonomous starts, simply ask the {@link SendableChooser} what the selected
* value is.

309
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
View File

@@ -1,309 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Set;
import java.util.function.BooleanSupplier;
/**
* A state machine representing a complete action to be performed by the robot. Commands are
* run by the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to
* build complicated multi-step actions without the need to roll the state machine logic themselves.
*
* <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
* specified explicitly from the command implementation.
*/
@SuppressWarnings("PMD.TooManyMethods")
public interface Command {
/**
* The initial subroutine of a command. Called once when the command is initially scheduled.
*/
default void initialize() {
}
/**
* The main body of a command. Called repeatedly while the command is scheduled.
*/
default void execute() {
}
/**
* The action to take when the command ends. Called when either the command finishes normally,
* or when it interrupted/canceled.
*
* @param interrupted whether the command was interrupted/canceled
*/
default void end(boolean interrupted) {
}
/**
* Whether the command has finished. Once a command finishes, the scheduler will call its
* end() method and un-schedule it.
*
* @return whether the command has finished.
*/
default boolean isFinished() {
return false;
}
/**
* Specifies the set of subsystems used by this command. Two commands cannot use the same
* subsystem at the same time. If the command is scheduled as interruptible and another
* command is scheduled that shares a requirement, the command will be interrupted. Else,
* the command will not be scheduled. If no subsystems are required, return an empty set.
*
* <p>Note: it is recommended that user implementations contain the requirements as a field,
* and return that field here, rather than allocating a new set every time this is called.
*
* @return the set of subsystems that are required
*/
Set<Subsystem> getRequirements();
/**
* Decorates this command with a timeout. If the specified timeout is exceeded before the command
* finishes normally, the command will be interrupted and un-scheduled. Note that the
* timeout only applies to the command returned by this method; the calling command is
* not itself changed.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param seconds the timeout duration
* @return the command with the timeout added
*/
default ParallelRaceGroup withTimeout(double seconds) {
return new ParallelRaceGroup(this, new WaitCommand(seconds));
}
/**
* Decorates this command with an interrupt condition. If the specified condition becomes true
* before the command finishes normally, the command will be interrupted and un-scheduled.
* Note that this only applies to the command returned by this method; the calling command
* is not itself changed.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param condition the interrupt condition
* @return the command with the interrupt condition added
*/
default ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
}
/**
* Decorates this command with a runnable to run before this command starts.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param toRun the Runnable to run
* @return the decorated command
*/
default SequentialCommandGroup beforeStarting(Runnable toRun) {
return new SequentialCommandGroup(new InstantCommand(toRun), this);
}
/**
* Decorates this command with a runnable to run after the command finishes.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param toRun the Runnable to run
* @return the decorated command
*/
default SequentialCommandGroup andThen(Runnable toRun) {
return new SequentialCommandGroup(this, new InstantCommand(toRun));
}
/**
* Decorates this command with a set of commands to run after it in sequence. Often more
* convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param next the commands to run next
* @return the decorated command
*/
default SequentialCommandGroup andThen(Command... next) {
SequentialCommandGroup group = new SequentialCommandGroup(this);
group.addCommands(next);
return group;
}
/**
* Decorates this command with a set of commands to run parallel to it, ending when the calling
* command ends and interrupting all the others. Often more convenient/less-verbose than
* constructing a new {@link ParallelDeadlineGroup} explicitly.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param parallel the commands to run in parallel
* @return the decorated command
*/
default ParallelDeadlineGroup deadlineWith(Command... parallel) {
return new ParallelDeadlineGroup(this, parallel);
}
/**
* Decorates this command with a set of commands to run parallel to it, ending when the last
* command ends. Often more convenient/less-verbose than constructing a new
* {@link ParallelCommandGroup} explicitly.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param parallel the commands to run in parallel
* @return the decorated command
*/
default ParallelCommandGroup alongWith(Command... parallel) {
ParallelCommandGroup group = new ParallelCommandGroup(this);
group.addCommands(parallel);
return group;
}
/**
* Decorates this command with a set of commands to run parallel to it, ending when the first
* command ends. Often more convenient/less-verbose than constructing a new
* {@link ParallelRaceGroup} explicitly.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @param parallel the commands to run in parallel
* @return the decorated command
*/
default ParallelRaceGroup raceWith(Command... parallel) {
ParallelRaceGroup group = new ParallelRaceGroup(this);
group.addCommands(parallel);
return group;
}
/**
* Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated
* command can still be interrupted or canceled.
*
* <p>Note: This decorator works by composing this command within a CommandGroup. The command
* cannot be used independently after being decorated, or be re-decorated with a different
* decorator, unless it is manually cleared from the list of grouped commands with
* {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
* further decorated without issue.
*
* @return the decorated command
*/
default PerpetualCommand perpetually() {
return new PerpetualCommand(this);
}
/**
* Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}.
* This is useful for "forking off" from command groups when the user does not wish to extend
* the command's requirements to the entire command group.
*
* @return the decorated command
*/
default ProxyScheduleCommand asProxy() {
return new ProxyScheduleCommand(this);
}
/**
* Schedules this command.
*
* @param interruptible whether this command can be interrupted by another command that
* shares one of its requirements
*/
default void schedule(boolean interruptible) {
CommandScheduler.getInstance().schedule(interruptible, this);
}
/**
* Schedules this command, defaulting to interruptible.
*/
default void schedule() {
schedule(true);
}
/**
* Cancels this command. Will call the command's interrupted() method.
* Commands will be canceled even if they are not marked as interruptible.
*/
default void cancel() {
CommandScheduler.getInstance().cancel(this);
}
/**
* Whether or not the command is currently scheduled. Note that this does not detect whether
* the command is being run by a CommandGroup, only whether it is directly being run by
* the scheduler.
*
* @return Whether the command is scheduled.
*/
default boolean isScheduled() {
return CommandScheduler.getInstance().isScheduled(this);
}
/**
* Whether the command requires a given subsystem. Named "hasRequirement" rather than "requires"
* to avoid confusion with
* {@link edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
* - this may be able to be changed in a few years.
*
* @param requirement the subsystem to inquire about
* @return whether the subsystem is required
*/
default boolean hasRequirement(Subsystem requirement) {
return getRequirements().contains(requirement);
}
/**
* Whether the given command should run when the robot is disabled. Override to return true
* if the command should run when disabled.
*
* @return whether the command should run when the robot is disabled
*/
default boolean runsWhenDisabled() {
return false;
}
/**
* Gets the name of this Command.
*
* @return Name
*/
default String getName() {
return this.getClass().getSimpleName();
}
}

102
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
View File

@@ -1,102 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.HashSet;
import java.util.Set;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* A {@link Sendable} base class for {@link Command}s.
*/
@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
public abstract class CommandBase implements Sendable, Command {
protected Set<Subsystem> m_requirements = new HashSet<>();
protected CommandBase() {
String name = getClass().getName();
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
}
/**
* Adds the specified requirements to the command.
*
* @param requirements the requirements to add
*/
public final void addRequirements(Subsystem... requirements) {
m_requirements.addAll(Set.of(requirements));
}
@Override
public Set<Subsystem> getRequirements() {
return m_requirements;
}
@Override
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Command.
*
* @param name name
*/
@Override
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Command.
*
* @return Subsystem name
*/
@Override
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Command.
*
* @param subsystem subsystem name
*/
@Override
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
*
* @param builder the builder used to construct this sendable
*/
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Command");
builder.addStringProperty(".name", this::getName, null);
builder.addBooleanProperty("running", this::isScheduled, value -> {
if (value) {
if (!isScheduled()) {
schedule();
}
} else {
if (isScheduled()) {
cancel();
}
}
});
builder.addBooleanProperty(".isParented",
() -> CommandGroupBase.getGroupedCommands().contains(this), null);
}
}

124
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
View File

@@ -1,124 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Collection;
import java.util.Collections;
import java.util.Set;
import java.util.WeakHashMap;
/**
* A base for CommandGroups. Statically tracks commands that have been allocated to groups to
* ensure those commands are not also used independently, which can result in inconsistent command
* state and unpredictable execution.
*/
public abstract class CommandGroupBase extends CommandBase implements Command {
private static final Set<Command> m_groupedCommands =
Collections.newSetFromMap(new WeakHashMap<>());
static void registerGroupedCommands(Command... commands) {
m_groupedCommands.addAll(Set.of(commands));
}
/**
* Clears the list of grouped commands, allowing all commands to be freely used again.
*
* <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not
* use this unless you fully understand what you are doing.
*/
public static void clearGroupedCommands() {
m_groupedCommands.clear();
}
/**
* Removes a single command from the list of grouped commands, allowing it to be freely used
* again.
*
* <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not
* use this unless you fully understand what you are doing.
*
* @param command the command to remove from the list of grouped commands
*/
public static void clearGroupedCommand(Command command) {
m_groupedCommands.remove(command);
}
/**
* Requires that the specified commands not have been already allocated to a CommandGroup. Throws
* an {@link IllegalArgumentException} if commands have been allocated.
*
* @param commands The commands to check
*/
public static void requireUngrouped(Command... commands) {
requireUngrouped(Set.of(commands));
}
/**
* Requires that the specified commands not have been already allocated to a CommandGroup. Throws
* an {@link IllegalArgumentException} if commands have been allocated.
*
* @param commands The commands to check
*/
public static void requireUngrouped(Collection<Command> commands) {
if (!Collections.disjoint(commands, getGroupedCommands())) {
throw new IllegalArgumentException("Commands cannot be added to more than one CommandGroup");
}
}
static Set<Command> getGroupedCommands() {
return m_groupedCommands;
}
/**
* Adds the given commands to the command group.
*
* @param commands The commands to add.
*/
public abstract void addCommands(Command... commands);
/**
* Factory method for {@link SequentialCommandGroup}, included for brevity/convenience.
*
* @param commands the commands to include
* @return the command group
*/
public static CommandGroupBase sequence(Command... commands) {
return new SequentialCommandGroup(commands);
}
/**
* Factory method for {@link ParallelCommandGroup}, included for brevity/convenience.
*
* @param commands the commands to include
* @return the command group
*/
public static CommandGroupBase parallel(Command... commands) {
return new ParallelCommandGroup(commands);
}
/**
* Factory method for {@link ParallelRaceGroup}, included for brevity/convenience.
*
* @param commands the commands to include
* @return the command group
*/
public static CommandGroupBase race(Command... commands) {
return new ParallelRaceGroup(commands);
}
/**
* Factory method for {@link ParallelDeadlineGroup}, included for brevity/convenience.
*
* @param deadline the deadline command
* @param commands the commands to include
* @return the command group
*/
public static CommandGroupBase deadline(Command deadline, Command... commands) {
return new ParallelDeadlineGroup(deadline, commands);
}
}

509
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View File

@@ -1,509 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.function.Consumer;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link
* CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands
* synchronously from the main loop. Subsystems should be registered with the scheduler using
* {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link
* Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
*/
@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
public final class CommandScheduler implements Sendable {
/**
* The Singleton Instance.
*/
private static CommandScheduler instance;
/**
* Returns the Scheduler instance.
*
* @return the instance
*/
public static synchronized CommandScheduler getInstance() {
if (instance == null) {
instance = new CommandScheduler();
}
return instance;
}
//A map from commands to their scheduling state. Also used as a set of the currently-running
//commands.
private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
//A map from required subsystems to their requiring commands. Also used as a set of the
//currently-required subsystems.
private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
//A map from subsystems registered with the scheduler to their default commands. Also used
//as a list of currently-registered subsystems.
private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>();
//The set of currently-registered buttons that will be polled every iteration.
private final Collection<Runnable> m_buttons = new LinkedHashSet<>();
private boolean m_disabled;
//NetworkTable entries for use in Sendable impl
private NetworkTableEntry m_namesEntry;
private NetworkTableEntry m_idsEntry;
private NetworkTableEntry m_cancelEntry;
//Lists of user-supplied actions to be executed on scheduling events for every command.
private final List<Consumer<Command>> m_initActions = new ArrayList<>();
private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
private final List<Consumer<Command>> m_interruptActions = new ArrayList<>();
private final List<Consumer<Command>> m_finishActions = new ArrayList<>();
// Flag and queues for avoiding ConcurrentModificationException if commands are
// scheduled/canceled during run
private boolean m_inRunLoop;
private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
private final List<Command> m_toCancel = new ArrayList<>();
CommandScheduler() {
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
SendableRegistry.addLW(this, "Scheduler");
}
/**
* Adds a button binding to the scheduler, which will be polled to schedule commands.
*
* @param button The button to add
*/
public void addButton(Runnable button) {
m_buttons.add(button);
}
/**
* Removes all button bindings from the scheduler.
*/
public void clearButtons() {
m_buttons.clear();
}
/**
* Initializes a given command, adds its requirements to the list, and performs the init actions.
*
* @param command The command to initialize
* @param interruptible Whether the command is interruptible
* @param requirements The command requirements
*/
private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
command.initialize();
CommandState scheduledCommand = new CommandState(interruptible);
m_scheduledCommands.put(command, scheduledCommand);
for (Consumer<Command> action : m_initActions) {
action.accept(command);
}
for (Subsystem requirement : requirements) {
m_requirements.put(requirement, command);
}
}
/**
* Schedules a command for execution. Does nothing if the command is already scheduled. If a
* command's requirements are not available, it will only be started if all the commands currently
* using those requirements have been scheduled as interruptible. If this is the case, they will
* be interrupted and the command will be scheduled.
*
* @param interruptible whether this command can be interrupted
* @param command the command to schedule
*/
@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
private void schedule(boolean interruptible, Command command) {
if (m_inRunLoop) {
m_toSchedule.put(command, interruptible);
return;
}
if (CommandGroupBase.getGroupedCommands().contains(command)) {
throw new IllegalArgumentException(
"A command that is part of a command group cannot be independently scheduled");
}
//Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
//run when disabled, or the command is already scheduled.
if (m_disabled || (RobotState.isDisabled() && !command.runsWhenDisabled())
|| m_scheduledCommands.containsKey(command)) {
return;
}
Set<Subsystem> requirements = command.getRequirements();
//Schedule the command if the requirements are not currently in-use.
if (Collections.disjoint(m_requirements.keySet(), requirements)) {
initCommand(command, interruptible, requirements);
} else {
//Else check if the requirements that are in use have all have interruptible commands,
//and if so, interrupt those commands and schedule the new command.
for (Subsystem requirement : requirements) {
if (m_requirements.containsKey(requirement)
&& !m_scheduledCommands.get(m_requirements.get(requirement)).isInterruptible()) {
return;
}
}
for (Subsystem requirement : requirements) {
if (m_requirements.containsKey(requirement)) {
cancel(m_requirements.get(requirement));
}
}
initCommand(command, interruptible, requirements);
}
}
/**
* Schedules multiple commands for execution. Does nothing if the command is already scheduled.
* If a command's requirements are not available, it will only be started if all the commands
* currently using those requirements have been scheduled as interruptible. If this is the case,
* they will be interrupted and the command will be scheduled.
*
* @param interruptible whether the commands should be interruptible
* @param commands the commands to schedule
*/
public void schedule(boolean interruptible, Command... commands) {
for (Command command : commands) {
schedule(interruptible, command);
}
}
/**
* Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
* if the command is already scheduled.
*
* @param commands the commands to schedule
*/
public void schedule(Command... commands) {
schedule(true, commands);
}
/**
* Runs a single iteration of the scheduler. The execution occurs in the following order:
*
* <p>Subsystem periodic methods are called.
*
* <p>Button bindings are polled, and new commands are scheduled from them.
*
* <p>Currently-scheduled commands are executed.
*
* <p>End conditions are checked on currently-scheduled commands, and commands that are finished
* have their end methods called and are removed.
*
* <p>Any subsystems not being used as requirements have their default methods started.
*/
@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
public void run() {
if (m_disabled) {
return;
}
//Run the periodic method of all registered subsystems.
for (Subsystem subsystem : m_subsystems.keySet()) {
subsystem.periodic();
}
//Poll buttons for new commands to add.
for (Runnable button : m_buttons) {
button.run();
}
m_inRunLoop = true;
//Run scheduled commands, remove finished commands.
for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
iterator.hasNext(); ) {
Command command = iterator.next();
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
command.end(true);
for (Consumer<Command> action : m_interruptActions) {
action.accept(command);
}
m_requirements.keySet().removeAll(command.getRequirements());
iterator.remove();
continue;
}
command.execute();
for (Consumer<Command> action : m_executeActions) {
action.accept(command);
}
if (command.isFinished()) {
command.end(false);
for (Consumer<Command> action : m_finishActions) {
action.accept(command);
}
iterator.remove();
m_requirements.keySet().removeAll(command.getRequirements());
}
}
m_inRunLoop = false;
//Schedule/cancel commands from queues populated during loop
for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
}
for (Command command : m_toCancel) {
cancel(command);
}
m_toSchedule.clear();
m_toCancel.clear();
//Add default commands for un-required registered subsystems.
for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
if (!m_requirements.containsKey(subsystemCommand.getKey())
&& subsystemCommand.getValue() != null) {
schedule(subsystemCommand.getValue());
}
}
}
/**
* Registers subsystems with the scheduler. This must be called for the subsystem's periodic
* block to run when the scheduler is run, and for the subsystem's default command to be
* scheduled. It is recommended to call this from the constructor of your subsystem
* implementations.
*
* @param subsystems the subsystem to register
*/
public void registerSubsystem(Subsystem... subsystems) {
for (Subsystem subsystem : subsystems) {
m_subsystems.put(subsystem, null);
}
}
/**
* Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic
* block called, and will not have its default command scheduled.
*
* @param subsystems the subsystem to un-register
*/
public void unregisterSubsystem(Subsystem... subsystems) {
m_subsystems.keySet().removeAll(Set.of(subsystems));
}
/**
* Sets the default command for a subsystem. Registers that subsystem if it is not already
* registered. Default commands will run whenever there is no other command currently scheduled
* that requires the subsystem. Default commands should be written to never end (i.e. their
* {@link Command#isFinished()} method should return false), as they would simply be re-scheduled
* if they do. Default commands must also require their subsystem.
*
* @param subsystem the subsystem whose default command will be set
* @param defaultCommand the default command to associate with the subsystem
*/
public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) {
if (!defaultCommand.getRequirements().contains(subsystem)) {
throw new IllegalArgumentException("Default commands must require their subsystem!");
}
if (defaultCommand.isFinished()) {
throw new IllegalArgumentException("Default commands should not end!");
}
m_subsystems.put(subsystem, defaultCommand);
}
/**
* Gets the default command associated with this subsystem. Null if this subsystem has no default
* command associated with it.
*
* @param subsystem the subsystem to inquire about
* @return the default command associated with the subsystem
*/
public Command getDefaultCommand(Subsystem subsystem) {
return m_subsystems.get(subsystem);
}
/**
* Cancels commands. The scheduler will only call the interrupted method of a canceled command,
* not the end method (though the interrupted method may itself call the end method). Commands
* will be canceled even if they are not scheduled as interruptible.
*
* @param commands the commands to cancel
*/
public void cancel(Command... commands) {
if (m_inRunLoop) {
m_toCancel.addAll(List.of(commands));
return;
}
for (Command command : commands) {
if (!m_scheduledCommands.containsKey(command)) {
continue;
}
command.end(true);
for (Consumer<Command> action : m_interruptActions) {
action.accept(command);
}
m_scheduledCommands.remove(command);
m_requirements.keySet().removeAll(command.getRequirements());
}
}
/**
* Cancels all commands that are currently scheduled.
*/
public void cancelAll() {
for (Command command : m_scheduledCommands.keySet()) {
cancel(command);
}
}
/**
* Returns the time since a given command was scheduled. Note that this only works on commands
* that are directly scheduled by the scheduler; it will not work on commands inside of
* commandgroups, as the scheduler does not see them.
*
* @param command the command to query
* @return the time since the command was scheduled, in seconds
*/
public double timeSinceScheduled(Command command) {
CommandState commandState = m_scheduledCommands.get(command);
if (commandState != null) {
return commandState.timeSinceInitialized();
} else {
return -1;
}
}
/**
* Whether the given commands are running. Note that this only works on commands that are
* directly scheduled by the scheduler; it will not work on commands inside of CommandGroups, as
* the scheduler does not see them.
*
* @param commands the command to query
* @return whether the command is currently scheduled
*/
public boolean isScheduled(Command... commands) {
return m_scheduledCommands.keySet().containsAll(Set.of(commands));
}
/**
* Returns the command currently requiring a given subsystem. Null if no command is currently
* requiring the subsystem
*
* @param subsystem the subsystem to be inquired about
* @return the command currently requiring the subsystem
*/
public Command requiring(Subsystem subsystem) {
return m_requirements.get(subsystem);
}
/**
* Disables the command scheduler.
*/
public void disable() {
m_disabled = true;
}
/**
* Enables the command scheduler.
*/
public void enable() {
m_disabled = false;
}
/**
* Adds an action to perform on the initialization of any command by the scheduler.
*
* @param action the action to perform
*/
public void onCommandInitialize(Consumer<Command> action) {
m_initActions.add(action);
}
/**
* Adds an action to perform on the execution of any command by the scheduler.
*
* @param action the action to perform
*/
public void onCommandExecute(Consumer<Command> action) {
m_executeActions.add(action);
}
/**
* Adds an action to perform on the interruption of any command by the scheduler.
*
* @param action the action to perform
*/
public void onCommandInterrupt(Consumer<Command> action) {
m_interruptActions.add(action);
}
/**
* Adds an action to perform on the finishing of any command by the scheduler.
*
* @param action the action to perform
*/
public void onCommandFinish(Consumer<Command> action) {
m_finishActions.add(action);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Scheduler");
m_namesEntry = builder.getEntry("Names");
m_idsEntry = builder.getEntry("Ids");
m_cancelEntry = builder.getEntry("Cancel");
builder.setUpdateTable(() -> {
if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
return;
}
Map<Double, Command> ids = new LinkedHashMap<>();
for (Command command : m_scheduledCommands.keySet()) {
ids.put((double) command.hashCode(), command);
}
double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
if (toCancel.length > 0) {
for (double hash : toCancel) {
cancel(ids.get(hash));
ids.remove(hash);
}
m_cancelEntry.setDoubleArray(new double[0]);
}
List<String> names = new ArrayList<>();
ids.values().forEach(command -> names.add(command.getName()));
m_namesEntry.setStringArray(names.toArray(new String[0]));
m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
});
}
}

44
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
View File

@@ -1,44 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.Timer;
/**
* Class that holds scheduling state for a command. Used internally by the
* {@link CommandScheduler}.
*/
class CommandState {
//The time since this command was initialized.
private double m_startTime = -1;
//Whether or not it is interruptible.
private final boolean m_interruptible;
CommandState(boolean interruptible) {
m_interruptible = interruptible;
startTiming();
startRunning();
}
private void startTiming() {
m_startTime = Timer.getFPGATimestamp();
}
synchronized void startRunning() {
m_startTime = -1;
}
boolean isInterruptible() {
return m_interruptible;
}
double timeSinceInitialized() {
return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
}
}

82
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
View File

@@ -1,82 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
/**
* Runs one of two commands, depending on the value of the given condition when this command is
* initialized. Does not actually schedule the selected command - rather, the command is run
* through this command; this ensures that the command will behave as expected if used as part of a
* CommandGroup. Requires the requirements of both commands, again to ensure proper functioning
* when used in a CommandGroup. If this is undesired, consider using {@link ScheduleCommand}.
*
* <p>As this command contains multiple component commands within it, it is technically a command
* group; the command instances that are passed to it cannot be added to any other groups, or
* scheduled individually.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class ConditionalCommand extends CommandBase {
private final Command m_onTrue;
private final Command m_onFalse;
private final BooleanSupplier m_condition;
private Command m_selectedCommand;
/**
* Creates a new ConditionalCommand.
*
* @param onTrue the command to run if the condition is true
* @param onFalse the command to run if the condition is false
* @param condition the condition to determine which command to run
*/
public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) {
requireUngrouped(onTrue, onFalse);
CommandGroupBase.registerGroupedCommands(onTrue, onFalse);
m_onTrue = onTrue;
m_onFalse = onFalse;
m_condition = requireNonNullParam(condition, "condition", "ConditionalCommand");
m_requirements.addAll(m_onTrue.getRequirements());
m_requirements.addAll(m_onFalse.getRequirements());
}
@Override
public void initialize() {
if (m_condition.getAsBoolean()) {
m_selectedCommand = m_onTrue;
} else {
m_selectedCommand = m_onFalse;
}
m_selectedCommand.initialize();
}
@Override
public void execute() {
m_selectedCommand.execute();
}
@Override
public void end(boolean interrupted) {
m_selectedCommand.end(interrupted);
}
@Override
public boolean isFinished() {
return m_selectedCommand.isFinished();
}
@Override
public boolean runsWhenDisabled() {
return m_onTrue.runsWhenDisabled() && m_onFalse.runsWhenDisabled();
}
}

65
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
View File

@@ -1,65 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that allows the user to pass in functions for each of the basic command methods through
* the constructor. Useful for inline definitions of complex commands - note, however, that if a
* command is beyond a certain complexity it is usually better practice to write a proper class for
* it than to inline it.
*/
public class FunctionalCommand extends CommandBase {
protected final Runnable m_onInit;
protected final Runnable m_onExecute;
protected final Consumer<Boolean> m_onEnd;
protected final BooleanSupplier m_isFinished;
/**
* Creates a new FunctionalCommand.
*
* @param onInit the function to run on command initialization
* @param onExecute the function to run on command execution
* @param onEnd the function to run on command end
* @param isFinished the function that determines whether the command has finished
* @param requirements the subsystems required by this command
*/
public FunctionalCommand(Runnable onInit, Runnable onExecute, Consumer<Boolean> onEnd,
BooleanSupplier isFinished, Subsystem... requirements) {
m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand");
m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand");
m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand");
m_isFinished = requireNonNullParam(isFinished, "isFinished", "FunctionalCommand");
addRequirements(requirements);
}
@Override
public void initialize() {
m_onInit.run();
}
@Override
public void execute() {
m_onExecute.run();
}
@Override
public void end(boolean interrupted) {
m_onEnd.accept(interrupted);
}
@Override
public boolean isFinished() {
return m_isFinished.getAsBoolean();
}
}

50
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
View File

@@ -1,50 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A Command that runs instantly; it will initialize, execute once, and end on the same
* iteration of the scheduler. Users can either pass in a Runnable and a set of requirements,
* or else subclass this command if desired.
*/
public class InstantCommand extends CommandBase {
private final Runnable m_toRun;
/**
* Creates a new InstantCommand that runs the given Runnable with the given requirements.
*
* @param toRun the Runnable to run
* @param requirements the subsystems required by this command
*/
public InstantCommand(Runnable toRun, Subsystem... requirements) {
m_toRun = requireNonNullParam(toRun, "toRun", "InstantCommand");
addRequirements(requirements);
}
/**
* Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg
* constructor to call implicitly from subclass constructors.
*/
public InstantCommand() {
m_toRun = () -> {
};
}
@Override
public void initialize() {
m_toRun.run();
}
@Override
public final boolean isFinished() {
return true;
}
}

49
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
View File

@@ -1,49 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj.Notifier;
/**
* A command that starts a notifier to run the given runnable periodically in a separate thread.
* Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
* {@link Command#withInterrupt(BooleanSupplier)} to give it one.
*
* <p>WARNING: Do not use this class unless you are confident in your ability to make the executed
* code thread-safe. If you do not know what "thread-safe" means, that is a good sign that
* you should not use this class.
*/
public class NotifierCommand extends CommandBase {
protected final Notifier m_notifier;
protected final double m_period;
/**
* Creates a new NotifierCommand.
*
* @param toRun the runnable for the notifier to run
* @param period the period at which the notifier should run, in seconds
* @param requirements the subsystems required by this command
*/
public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
m_notifier = new Notifier(toRun);
m_period = period;
addRequirements(requirements);
}
@Override
public void initialize() {
m_notifier.startPeriodic(m_period);
}
@Override
public void end(boolean interrupted) {
m_notifier.stop();
}
}

92
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
View File

@@ -1,92 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Set;
import java.util.function.DoubleConsumer;
import java.util.function.DoubleSupplier;
import edu.wpi.first.wpilibj.controller.PIDController;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that controls an output with a {@link PIDController}. Runs forever by default - to add
* exit conditions and/or other behavior, subclass this class. The controller calculation and
* output are performed synchronously in the command's execute() method.
*/
public class PIDCommand extends CommandBase {
protected final PIDController m_controller;
protected DoubleSupplier m_measurement;
protected DoubleSupplier m_setpoint;
protected DoubleConsumer m_useOutput;
/**
* Creates a new PIDCommand, which controls the given output with a PIDController.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param setpointSource the controller's setpoint
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
DoubleSupplier setpointSource, DoubleConsumer useOutput,
Subsystem... requirements) {
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
m_controller = controller;
m_useOutput = useOutput;
m_measurement = measurementSource;
m_setpoint = setpointSource;
m_requirements.addAll(Set.of(requirements));
}
/**
* Creates a new PIDCommand, which controls the given output with a PIDController.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param setpoint the controller's setpoint
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
double setpoint, DoubleConsumer useOutput,
Subsystem... requirements) {
this(controller, measurementSource, () -> setpoint, useOutput, requirements);
}
@Override
public void initialize() {
m_controller.reset();
}
@Override
public void execute() {
m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
m_setpoint.getAsDouble()));
}
@Override
public void end(boolean interrupted) {
m_useOutput.accept(0);
}
/**
* Returns the PIDController used by the command.
*
* @return The PIDController
*/
public PIDController getController() {
return m_controller;
}
}

79
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
View File

@@ -1,79 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.controller.PIDController;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A subsystem that uses a {@link PIDController} to control an output. The controller is run
* synchronously from the subsystem's periodic() method.
*/
public abstract class PIDSubsystem extends SubsystemBase {
protected final PIDController m_controller;
protected boolean m_enabled;
/**
* Creates a new PIDSubsystem.
*
* @param controller the PIDController to use
*/
public PIDSubsystem(PIDController controller) {
requireNonNullParam(controller, "controller", "PIDSubsystem");
m_controller = controller;
}
@Override
public void periodic() {
if (m_enabled) {
useOutput(m_controller.calculate(getMeasurement(), getSetpoint()));
}
}
public PIDController getController() {
return m_controller;
}
/**
* Uses the output from the PIDController.
*
* @param output the output of the PIDController
*/
public abstract void useOutput(double output);
/**
* Returns the reference (setpoint) used by the PIDController.
*
* @return the reference (setpoint) to be used by the controller
*/
public abstract double getSetpoint();
/**
* Returns the measurement of the process variable used by the PIDController.
*
* @return the measurement of the process variable
*/
public abstract double getMeasurement();
/**
* Enables the PID control. Resets the controller.
*/
public void enable() {
m_enabled = true;
m_controller.reset();
}
/**
* Disables the PID control. Sets output to zero.
*/
public void disable() {
m_enabled = false;
useOutput(0);
}
}

99
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
View File

@@ -1,99 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Collections;
import java.util.HashMap;
import java.util.Map;
/**
* A CommandGroup that runs a set of commands in parallel, ending when the last command ends.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class ParallelCommandGroup extends CommandGroupBase {
//maps commands in this group to whether they are still running
private final Map<Command, Boolean> m_commands = new HashMap<>();
private boolean m_runWhenDisabled = true;
/**
* Creates a new ParallelCommandGroup. The given commands will be executed simultaneously.
* The command group will finish when the last command finishes. If the CommandGroup is
* interrupted, only the commands that are still running will be interrupted.
*
* @param commands the commands to include in this group.
*/
public ParallelCommandGroup(Command... commands) {
addCommands(commands);
}
@Override
public final void addCommands(Command... commands) {
requireUngrouped(commands);
if (m_commands.containsValue(true)) {
throw new IllegalStateException(
"Commands cannot be added to a CommandGroup while the group is running");
}
registerGroupedCommands(commands);
for (Command command : commands) {
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+ "require the same subsystems");
}
m_commands.put(command, false);
m_requirements.addAll(command.getRequirements());
m_runWhenDisabled &= command.runsWhenDisabled();
}
}
@Override
public void initialize() {
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
commandRunning.getKey().initialize();
commandRunning.setValue(true);
}
}
@Override
public void execute() {
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
if (!commandRunning.getValue()) {
continue;
}
commandRunning.getKey().execute();
if (commandRunning.getKey().isFinished()) {
commandRunning.getKey().end(false);
commandRunning.setValue(false);
}
}
}
@Override
public void end(boolean interrupted) {
if (interrupted) {
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
if (commandRunning.getValue()) {
commandRunning.getKey().end(true);
}
}
}
}
@Override
public boolean isFinished() {
return !m_commands.values().contains(true);
}
@Override
public boolean runsWhenDisabled() {
return m_runWhenDisabled;
}
}

118
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
View File

@@ -1,118 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Collections;
import java.util.HashMap;
import java.util.Map;
/**
* A CommandGroup that runs a set of commands in parallel, ending only when a specific command
* (the "deadline") ends, interrupting all other commands that are still running at that point.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class ParallelDeadlineGroup extends CommandGroupBase {
//maps commands in this group to whether they are still running
private final Map<Command, Boolean> m_commands = new HashMap<>();
private boolean m_runWhenDisabled = true;
private Command m_deadline;
/**
* Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
* executed simultaneously. The CommandGroup will finish when the deadline finishes,
* interrupting all other still-running commands. If the CommandGroup is interrupted, only
* the commands still running will be interrupted.
*
* @param deadline the command that determines when the group ends
* @param commands the commands to be executed
*/
public ParallelDeadlineGroup(Command deadline, Command... commands) {
m_deadline = deadline;
addCommands(commands);
if (!m_commands.containsKey(deadline)) {
addCommands(deadline);
}
}
/**
* Sets the deadline to the given command. The deadline is added to the group if it is not
* already contained.
*
* @param deadline the command that determines when the group ends
*/
public void setDeadline(Command deadline) {
if (!m_commands.containsKey(deadline)) {
addCommands(deadline);
}
m_deadline = deadline;
}
@Override
public final void addCommands(Command... commands) {
requireUngrouped(commands);
if (m_commands.containsValue(true)) {
throw new IllegalStateException(
"Commands cannot be added to a CommandGroup while the group is running");
}
registerGroupedCommands(commands);
for (Command command : commands) {
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+ "require the same subsystems");
}
m_commands.put(command, false);
m_requirements.addAll(command.getRequirements());
m_runWhenDisabled &= command.runsWhenDisabled();
}
}
@Override
public void initialize() {
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
commandRunning.getKey().initialize();
commandRunning.setValue(true);
}
}
@Override
public void execute() {
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
if (!commandRunning.getValue()) {
continue;
}
commandRunning.getKey().execute();
if (commandRunning.getKey().isFinished()) {
commandRunning.getKey().end(false);
commandRunning.setValue(false);
}
}
}
@Override
public void end(boolean interrupted) {
for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
if (commandRunning.getValue()) {
commandRunning.getKey().end(true);
}
}
}
@Override
public boolean isFinished() {
return m_deadline.isFinished();
}
@Override
public boolean runsWhenDisabled() {
return m_runWhenDisabled;
}
}

92
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
View File

@@ -1,92 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Collections;
import java.util.HashSet;
import java.util.Set;
/**
* A CommandGroup that runs a set of commands in parallel, ending when any one of the commands ends
* and interrupting all the others.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class ParallelRaceGroup extends CommandGroupBase {
private final Set<Command> m_commands = new HashSet<>();
private boolean m_runWhenDisabled = true;
private boolean m_finished = true;
/**
* Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and
* will "race to the finish" - the first command to finish ends the entire command, with all other
* commands being interrupted.
*
* @param commands the commands to include in this group.
*/
public ParallelRaceGroup(Command... commands) {
addCommands(commands);
}
@Override
public final void addCommands(Command... commands) {
requireUngrouped(commands);
if (!m_finished) {
throw new IllegalStateException(
"Commands cannot be added to a CommandGroup while the group is running");
}
registerGroupedCommands(commands);
for (Command command : commands) {
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+ " require the same subsystems");
}
m_commands.add(command);
m_requirements.addAll(command.getRequirements());
m_runWhenDisabled &= command.runsWhenDisabled();
}
}
@Override
public void initialize() {
m_finished = false;
for (Command command : m_commands) {
command.initialize();
}
}
@Override
public void execute() {
for (Command command : m_commands) {
command.execute();
if (command.isFinished()) {
m_finished = true;
}
}
}
@Override
public void end(boolean interrupted) {
for (Command command : m_commands) {
command.end(!command.isFinished());
}
}
@Override
public boolean isFinished() {
return m_finished;
}
@Override
public boolean runsWhenDisabled() {
return m_runWhenDisabled;
}
}

56
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
View File

@@ -1,56 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands;
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
/**
* A command that runs another command in perpetuity, ignoring that command's end conditions. While
* this class does not extend {@link CommandGroupBase}, it is still considered a CommandGroup, as it
* allows one to compose another command within it; the command instances that are passed to it
* cannot be added to any other groups, or scheduled individually.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class PerpetualCommand extends CommandBase {
protected final Command m_command;
/**
* Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that
* command's end conditions, unless this command itself is interrupted.
*
* @param command the command to run perpetually
*/
public PerpetualCommand(Command command) {
requireUngrouped(command);
registerGroupedCommands(command);
m_command = command;
m_requirements.addAll(command.getRequirements());
}
@Override
public void initialize() {
m_command.initialize();
}
@Override
public void execute() {
m_command.execute();
}
@Override
public void end(boolean interrupted) {
m_command.end(interrupted);
}
@Override
public boolean runsWhenDisabled() {
return m_command.runsWhenDisabled();
}
}

27
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
View File

@@ -1,27 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
/**
* A command that prints a string when initialized.
*/
public class PrintCommand extends InstantCommand {
/**
* Creates a new a PrintCommand.
*
* @param message the message to print
*/
public PrintCommand(String message) {
super(() -> System.out.println(message));
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}

138
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
View File

@@ -1,138 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Set;
import java.util.function.BiConsumer;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that controls an output with a {@link ProfiledPIDController}. Runs forever by
* default - to add
* exit conditions and/or other behavior, subclass this class. The controller calculation and
* output are performed synchronously in the command's execute() method.
*/
public class ProfiledPIDCommand extends CommandBase {
protected final ProfiledPIDController m_controller;
protected DoubleSupplier m_measurement;
protected Supplier<State> m_goal;
protected BiConsumer<Double, State> m_useOutput;
/**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
* Goal velocity is specified.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param goalSource the controller's goal
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
m_controller = controller;
m_useOutput = useOutput;
m_measurement = measurementSource;
m_goal = goalSource;
m_requirements.addAll(Set.of(requirements));
}
/**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
* Goal velocity is implicitly zero.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param goalSource the controller's goal
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
DoubleSupplier goalSource, BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
m_controller = controller;
m_useOutput = useOutput;
m_measurement = measurementSource;
m_goal = () -> new State(goalSource.getAsDouble(), 0);
m_requirements.addAll(Set.of(requirements));
}
/**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
* velocity is specified.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param goal the controller's goal
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
State goal, BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
this(controller, measurementSource, () -> goal, useOutput, requirements);
}
/**
* Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
* velocity is implicitly zero.
*
* @param controller the controller that controls the output.
* @param measurementSource the measurement of the process variable
* @param goal the controller's goal
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
double goal, BiConsumer<Double, State> useOutput,
Subsystem... requirements) {
this(controller, measurementSource, () -> goal, useOutput, requirements);
}
@Override
public void initialize() {
m_controller.reset();
}
@Override
public void execute() {
m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
m_controller.getSetpoint());
}
@Override
public void end(boolean interrupted) {
m_useOutput.accept(0., new State());
}
/**
* Returns the ProfiledPIDController used by the command.
*
* @return The ProfiledPIDController
*/
public ProfiledPIDController getController() {
return m_controller;
}
}

81
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
View File

@@ -1,81 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is
* run synchronously from the subsystem's periodic() method.
*/
public abstract class ProfiledPIDSubsystem extends SubsystemBase {
protected final ProfiledPIDController m_controller;
protected boolean m_enabled;
/**
* Creates a new ProfiledPIDSubsystem.
*
* @param controller the ProfiledPIDController to use
*/
public ProfiledPIDSubsystem(ProfiledPIDController controller) {
requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
m_controller = controller;
}
@Override
public void periodic() {
if (m_enabled) {
useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
}
}
public ProfiledPIDController getController() {
return m_controller;
}
/**
* Uses the output from the ProfiledPIDController.
*
* @param output the output of the ProfiledPIDController
* @param goal the goal state of the ProfiledPIDController, for feedforward
*/
public abstract void useOutput(double output, State goal);
/**
* Returns the goal used by the ProfiledPIDController.
*
* @return the goal to be used by the controller
*/
public abstract State getGoal();
/**
* Returns the measurement of the process variable used by the ProfiledPIDController.
*
* @return the measurement of the process variable
*/
public abstract double getMeasurement();
/**
* Enables the PID control. Resets the controller.
*/
public void enable() {
m_enabled = true;
m_controller.reset();
}
/**
* Disables the PID control. Sets output to zero.
*/
public void disable() {
m_enabled = false;
useOutput(0, new State());
}
}

59
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
View File

@@ -1,59 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Set;
/**
* Schedules the given commands when this command is initialized, and ends when all the commands are
* no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted,
* it will cancel all of the commands.
*/
public class ProxyScheduleCommand extends CommandBase {
private final Set<Command> m_toSchedule;
private boolean m_finished;
/**
* Creates a new ProxyScheduleCommand that schedules the given commands when initialized,
* and ends when they are all no longer scheduled.
*
* @param toSchedule the commands to schedule
*/
public ProxyScheduleCommand(Command... toSchedule) {
m_toSchedule = Set.of(toSchedule);
}
@Override
public void initialize() {
for (Command command : m_toSchedule) {
command.schedule();
}
}
@Override
public void end(boolean interrupted) {
if (interrupted) {
for (Command command : m_toSchedule) {
command.cancel();
}
}
}
@Override
public void execute() {
m_finished = true;
for (Command command : m_toSchedule) {
m_finished &= !command.isScheduled();
}
}
@Override
public boolean isFinished() {
return m_finished;
}
}

231
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -1,231 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BiConsumer;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
* {@link Trajectory} with a differential drive.
*
* <p>The command handles trajectory-following, PID calculations, and feedforwards internally. This
* is intended to be a more-or-less "complete solution" that can be used by teams without a great
* deal of controls expertise.
*
* <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
* PID functionality of a "smart" motor controller) may use the secondary constructor that omits
* the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE
* controller.
*/
@SuppressWarnings("PMD.TooManyFields")
public class RamseteCommand extends CommandBase {
private final Timer m_timer = new Timer();
private DifferentialDriveWheelSpeeds m_prevSpeeds;
private double m_prevTime;
private final boolean m_usePID;
private final Trajectory m_trajectory;
private final Supplier<Pose2d> m_pose;
private final RamseteController m_follower;
private final double m_ks;
private final double m_kv;
private final double m_ka;
private final DifferentialDriveKinematics m_kinematics;
private final DoubleSupplier m_leftSpeed;
private final DoubleSupplier m_rightSpeed;
private final PIDController m_leftController;
private final PIDController m_rightController;
private final BiConsumer<Double, Double> m_output;
/**
* Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
* PID control and feedforward are handled internally, and outputs are scaled -1 to 1 for easy
* consumption by speed controllers.
*
* <p>Note: The controller will *not* set the outputVolts to zero upon completion of the path -
* this
* is left to the user, since it is not appropriate for paths with nonstationary endstates.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param controller The RAMSETE controller used to follow the trajectory.
* @param ksVolts Constant feedforward term for the robot drive.
* @param kvVoltSecondsPerMeter Velocity-proportional feedforward term for the robot
* drive.
* @param kaVoltSecondsSquaredPerMeter Acceleration-proportional feedforward term for the robot
* drive.
* @param kinematics The kinematics for the robot drivetrain.
* @param leftWheelSpeedMetersPerSecond A function that supplies the speed of the left side of
* the robot drive.
* @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of
* the robot drive.
* @param leftController The PIDController for the left side of the robot drive.
* @param rightController The PIDController for the right side of the robot drive.
* @param outputVolts A function that consumes the computed left and right
* outputs (in volts) for the robot drive.
* @param requirements The subsystems to require.
*/
@SuppressWarnings("PMD.ExcessiveParameterList")
public RamseteCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
RamseteController controller,
double ksVolts,
double kvVoltSecondsPerMeter,
double kaVoltSecondsSquaredPerMeter,
DifferentialDriveKinematics kinematics,
DoubleSupplier leftWheelSpeedMetersPerSecond,
DoubleSupplier rightWheelSpeedMetersPerSecond,
PIDController leftController,
PIDController rightController,
BiConsumer<Double, Double> outputVolts,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
m_ks = ksVolts;
m_kv = kvVoltSecondsPerMeter;
m_ka = kaVoltSecondsSquaredPerMeter;
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
"leftWheelSpeedMetersPerSecond",
"RamseteCommand");
m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond,
"rightWheelSpeedMetersPerSecond",
"RamseteCommand");
m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
m_usePID = true;
addRequirements(requirements);
}
/**
* Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
* Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds
* from the RAMSETE controller, and will need to be converted into a usable form by the user.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param follower The RAMSETE follower used to follow the trajectory.
* @param kinematics The kinematics for the robot drivetrain.
* @param outputMetersPerSecond A function that consumes the computed left and right
* wheel speeds.
* @param requirements The subsystems to require.
*/
public RamseteCommand(Trajectory trajectory,
Supplier<Pose2d> pose,
RamseteController follower,
DifferentialDriveKinematics kinematics,
BiConsumer<Double, Double> outputMetersPerSecond,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
m_ks = 0;
m_kv = 0;
m_ka = 0;
m_leftSpeed = null;
m_rightSpeed = null;
m_leftController = null;
m_rightController = null;
m_usePID = false;
addRequirements(requirements);
}
@Override
public void initialize() {
m_prevTime = 0;
var initialState = m_trajectory.sample(0);
m_prevSpeeds = m_kinematics.toWheelSpeeds(
new ChassisSpeeds(initialState.velocityMetersPerSecond,
0,
initialState.curvatureRadPerMeter
* initialState.velocityMetersPerSecond));
m_timer.reset();
m_timer.start();
if (m_usePID) {
m_leftController.reset();
m_rightController.reset();
}
}
@Override
public void execute() {
double curTime = m_timer.get();
double dt = curTime - m_prevTime;
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
double leftOutput;
double rightOutput;
if (m_usePID) {
double leftFeedforward =
m_ks * Math.signum(leftSpeedSetpoint)
+ m_kv * leftSpeedSetpoint
+ m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt;
double rightFeedforward =
m_ks * Math.signum(rightSpeedSetpoint)
+ m_kv * rightSpeedSetpoint
+ m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt;
leftOutput = leftFeedforward
+ m_leftController.calculate(m_leftSpeed.getAsDouble(),
leftSpeedSetpoint);
rightOutput = rightFeedforward
+ m_rightController.calculate(m_rightSpeed.getAsDouble(),
rightSpeedSetpoint);
} else {
leftOutput = leftSpeedSetpoint;
rightOutput = rightSpeedSetpoint;
}
m_output.accept(leftOutput, rightOutput);
m_prevTime = curTime;
m_prevSpeeds = targetWheelSpeeds;
}
@Override
public void end(boolean interrupted) {
m_timer.stop();
}
@Override
public boolean isFinished() {
return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
}
}

39
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
View File

@@ -1,39 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that runs a Runnable continuously. Has no end condition as-is;
* either subclass it or use {@link Command#withTimeout(double)} or
* {@link Command#withInterrupt(BooleanSupplier)} to give it one. If you only wish
* to execute a Runnable once, use {@link InstantCommand}.
*/
public class RunCommand extends CommandBase {
protected final Runnable m_toRun;
/**
* Creates a new RunCommand. The Runnable will be run continuously until the command
* ends. Does not run when disabled.
*
* @param toRun the Runnable to run
* @param requirements the subsystems to require
*/
public RunCommand(Runnable toRun, Subsystem... requirements) {
m_toRun = requireNonNullParam(toRun, "toRun", "RunCommand");
addRequirements(requirements);
}
@Override
public void execute() {
m_toRun.run();
}
}

45
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
View File

@@ -1,45 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Set;
/**
* Schedules the given commands when this command is initialized. Useful for forking off from
* CommandGroups. Note that if run from a CommandGroup, the group will not know about the status
* of the scheduled commands, and will treat this command as finishing instantly.
*/
public class ScheduleCommand extends CommandBase {
private final Set<Command> m_toSchedule;
/**
* Creates a new ScheduleCommand that schedules the given commands when initialized.
*
* @param toSchedule the commands to schedule
*/
public ScheduleCommand(Command... toSchedule) {
m_toSchedule = Set.of(toSchedule);
}
@Override
public void initialize() {
for (Command command : m_toSchedule) {
command.schedule();
}
}
@Override
public boolean isFinished() {
return true;
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}

110
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
View File

@@ -1,110 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.Map;
import java.util.function.Supplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
/**
* Runs one of a selection of commands, either using a selector and a key to command mapping, or a
* supplier that returns the command directly at runtime. Does not actually schedule the selected
* command - rather, the command is run through this command; this ensures that the command will
* behave as expected if used as part of a CommandGroup. Requires the requirements of all included
* commands, again to ensure proper functioning when used in a CommandGroup. If this is undesired,
* consider using {@link ScheduleCommand}.
*
* <p>As this command contains multiple component commands within it, it is technically a command
* group; the command instances that are passed to it cannot be added to any other groups, or
* scheduled individually.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class SelectCommand extends CommandBase {
private final Map<Object, Command> m_commands;
private final Supplier<Object> m_selector;
private final Supplier<Command> m_toRun;
private Command m_selectedCommand;
/**
* Creates a new selectcommand.
*
* @param commands the map of commands to choose from
* @param selector the selector to determine which command to run
*/
public SelectCommand(Map<Object, Command> commands, Supplier<Object> selector) {
requireUngrouped(commands.values());
CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{}));
m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
m_toRun = null;
for (Command command : m_commands.values()) {
m_requirements.addAll(command.getRequirements());
}
}
/**
* Creates a new selectcommand.
*
* @param toRun a supplier providing the command to run
*/
public SelectCommand(Supplier<Command> toRun) {
m_commands = null;
m_selector = null;
m_toRun = requireNonNullParam(toRun, "toRun", "SelectCommand");
}
@Override
public void initialize() {
if (m_selector != null) {
if (!m_commands.keySet().contains(m_selector.get())) {
m_selectedCommand = new PrintCommand(
"SelectCommand selector value does not correspond to" + " any command!");
return;
}
m_selectedCommand = m_commands.get(m_selector.get());
} else {
m_selectedCommand = m_toRun.get();
}
m_selectedCommand.initialize();
}
@Override
public void execute() {
m_selectedCommand.execute();
}
@Override
public void end(boolean interrupted) {
m_selectedCommand.end(interrupted);
}
@Override
public boolean isFinished() {
return m_selectedCommand.isFinished();
}
@Override
public boolean runsWhenDisabled() {
if (m_commands != null) {
boolean runsWhenDisabled = true;
for (Command command : m_commands.values()) {
runsWhenDisabled &= command.runsWhenDisabled();
}
return runsWhenDisabled;
} else {
return m_toRun.get().runsWhenDisabled();
}
}
}

97
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
View File

@@ -1,97 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.ArrayList;
import java.util.List;
/**
* A CommandGroups that runs a list of commands in sequence.
*
* <p>As a rule, CommandGroups require the union of the requirements of their component commands.
*/
public class SequentialCommandGroup extends CommandGroupBase {
private final List<Command> m_commands = new ArrayList<>();
private int m_currentCommandIndex = -1;
private boolean m_runWhenDisabled = true;
/**
* Creates a new SequentialCommandGroup. The given commands will be run sequentially, with
* the CommandGroup finishing when the last command finishes.
*
* @param commands the commands to include in this group.
*/
public SequentialCommandGroup(Command... commands) {
addCommands(commands);
}
@Override
public final void addCommands(Command... commands) {
requireUngrouped(commands);
if (m_currentCommandIndex != -1) {
throw new IllegalStateException(
"Commands cannot be added to a CommandGroup while the group is running");
}
registerGroupedCommands(commands);
for (Command command : commands) {
m_commands.add(command);
m_requirements.addAll(command.getRequirements());
m_runWhenDisabled &= command.runsWhenDisabled();
}
}
@Override
public void initialize() {
m_currentCommandIndex = 0;
if (!m_commands.isEmpty()) {
m_commands.get(0).initialize();
}
}
@Override
public void execute() {
if (m_commands.isEmpty()) {
return;
}
Command currentCommand = m_commands.get(m_currentCommandIndex);
currentCommand.execute();
if (currentCommand.isFinished()) {
currentCommand.end(false);
m_currentCommandIndex++;
if (m_currentCommandIndex < m_commands.size()) {
m_commands.get(m_currentCommandIndex).initialize();
}
}
}
@Override
public void end(boolean interrupted) {
if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1
&& m_currentCommandIndex < m_commands.size()
) {
m_commands.get(m_currentCommandIndex).end(true);
}
m_currentCommandIndex = -1;
}
@Override
public boolean isFinished() {
return m_currentCommandIndex == m_commands.size();
}
@Override
public boolean runsWhenDisabled() {
return m_runWhenDisabled;
}
}

48
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
View File

@@ -1,48 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that runs a given runnable when it is initalized, and another runnable when it ends.
* Useful for running and then stopping a motor, or extending and then retracting a solenoid.
* Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
* {@link Command#withInterrupt(BooleanSupplier)} to give it one.
*/
public class StartEndCommand extends CommandBase {
protected final Runnable m_onInit;
protected final Runnable m_onEnd;
/**
* Creates a new StartEndCommand. Will run the given runnables when the command starts and when
* it ends.
*
* @param onInit the Runnable to run on command init
* @param onEnd the Runnable to run on command end
* @param requirements the subsystems required by this command
*/
public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) {
m_onInit = requireNonNullParam(onInit, "onInit", "StartEndCommand");
m_onEnd = requireNonNullParam(onEnd, "onEnd", "StartEndCommand");
addRequirements(requirements);
}
@Override
public void initialize() {
m_onInit.run();
}
@Override
public void end(boolean interrupted) {
m_onEnd.run();
}
}

76
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
View File

@@ -1,76 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
/**
* A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
* framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc) and
* provide methods through which they can be used by {@link Command}s. Subsystems are used by the
* {@link CommandScheduler}'s resource management system to ensure multiple robot actions are not
* "fighting" over the same hardware; Commands that use a subsystem should include that subsystem
* in their {@link Command#getRequirements()} method, and resources used within a subsystem should
* generally remain encapsulated and not be shared by other parts of the robot.
*
* <p>Subsystems must be registered with the scheduler with the
* {@link CommandScheduler#registerSubsystem(Subsystem...)} method in order for the
* {@link Subsystem#periodic()} method to be called. It is recommended that this method be called
* from the constructor of users' Subsystem implementations. The {@link SubsystemBase}
* class offers a simple base for user implementations that handles this.
*/
public interface Subsystem {
/**
* This method is called periodically by the {@link CommandScheduler}. Useful for updating
* subsystem-specific state that you don't want to offload to a {@link Command}. Teams should
* try to be consistent within their own codebases about which responsibilities will be handled
* by Commands, and which will be handled here.
*/
default void periodic() {
}
/**
* Sets the default {@link Command} of the subsystem. The default command will be
* automatically scheduled when no other commands are scheduled that require the subsystem.
* Default commands should generally not end on their own, i.e. their {@link Command#isFinished()}
* method should always return false. Will automatically register this subsystem with the
* {@link CommandScheduler}.
*
* @param defaultCommand the default command to associate with this subsystem
*/
default void setDefaultCommand(Command defaultCommand) {
CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
}
/**
* Gets the default command for this subsystem. Returns null if no default command is
* currently associated with the subsystem.
*
* @return the default command associated with this subsystem
*/
default Command getDefaultCommand() {
return CommandScheduler.getInstance().getDefaultCommand(this);
}
/**
* Returns the command currently running on this subsystem. Returns null if no command is
* currently scheduled that requires this subsystem.
*
* @return the scheduled command currently requiring this subsystem
*/
default Command getCurrentCommand() {
return CommandScheduler.getInstance().requiring(this);
}
/**
* Registers this subsystem with the {@link CommandScheduler}, allowing its
* {@link Subsystem#periodic()} method to be called when the scheduler runs.
*/
default void register() {
CommandScheduler.getInstance().registerSubsystem(this);
}
}

93
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
View File

@@ -1,93 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* A base for subsystems that handles registration in the constructor, and provides a more intuitive
* method for setting the default command.
*/
public abstract class SubsystemBase implements Subsystem, Sendable {
/**
* Constructor.
*/
public SubsystemBase() {
String name = this.getClass().getSimpleName();
name = name.substring(name.lastIndexOf('.') + 1);
SendableRegistry.addLW(this, name, name);
CommandScheduler.getInstance().registerSubsystem(this);
}
/**
* Gets the name of this Subsystem.
*
* @return Name
*/
@Override
public String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this Subsystem.
*
* @param name name
*/
@Override
public void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Gets the subsystem name of this Subsystem.
*
* @return Subsystem name
*/
@Override
public String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this Subsystem.
*
* @param subsystem subsystem name
*/
@Override
public void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Associates a {@link Sendable} with this Subsystem.
* Also update the child's name.
*
* @param name name to give child
* @param child sendable
*/
public void addChild(String name, Sendable child) {
SendableRegistry.addLW(child, getSubsystem(), name);
SendableRegistry.addChild(this, child);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Subsystem");
builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
builder.addStringProperty(".default",
() -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", null);
builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
builder.addStringProperty(".command",
() -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none", null);
}
}

64
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
View File

@@ -1,64 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.Consumer;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism
* motion.
*/
public class TrapezoidProfileCommand extends CommandBase {
private final TrapezoidProfile m_profile;
private final Consumer<State> m_output;
private final Timer m_timer = new Timer();
/**
* Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
* Output will be piped to the provided consumer function.
*
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param requirements The subsystems required by this command.
*/
public TrapezoidProfileCommand(TrapezoidProfile profile,
Consumer<State> output,
Subsystem... requirements) {
m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
addRequirements(requirements);
}
@Override
public void initialize() {
m_timer.reset();
m_timer.start();
}
@Override
public void execute() {
m_output.accept(m_profile.calculate(m_timer.get()));
}
@Override
public void end(boolean interrupted) {
m_timer.stop();
}
@Override
public boolean isFinished() {
return m_timer.hasPeriodPassed(m_profile.totalTime());
}
}

51
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
View File

@@ -1,51 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* A command that does nothing but takes a specified amount of time to finish. Useful for
* CommandGroups. Can also be subclassed to make a command with an internal timer.
*/
public class WaitCommand extends CommandBase {
protected Timer m_timer = new Timer();
private final double m_duration;
/**
* Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
*
* @param seconds the time to wait, in seconds
*/
public WaitCommand(double seconds) {
m_duration = seconds;
SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
}
@Override
public void initialize() {
m_timer.reset();
m_timer.start();
}
@Override
public void end(boolean interrupted) {
m_timer.stop();
}
@Override
public boolean isFinished() {
return m_timer.hasPeriodPassed(m_duration);
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}

55
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
View File

@@ -1,55 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj.Timer;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A command that does nothing but ends after a specified match time or condition. Useful for
* CommandGroups.
*/
public class WaitUntilCommand extends CommandBase {
private final BooleanSupplier m_condition;
/**
* Creates a new WaitUntilCommand that ends after a given condition becomes true.
*
* @param condition the condition to determine when to end
*/
public WaitUntilCommand(BooleanSupplier condition) {
m_condition = requireNonNullParam(condition, "condition", "WaitUntilCommand");
}
/**
* Creates a new WaitUntilCommand that ends after a given match time.
*
* <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT
* guarantee that the time at which the action is performed will be judged to be legal by the
* referees. When in doubt, add a safety factor or time the action manually.
*
* @param time the match time after which to end, in seconds
*/
public WaitUntilCommand(double time) {
this(() -> Timer.getMatchTime() - time > 0);
}
@Override
public boolean isFinished() {
return m_condition.getAsBoolean();
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}

211
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
View File

@@ -1,211 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command.button;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
/**
* This class provides an easy way to link commands to OI inputs.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger
* button of a joystick to a "score" command.
*
* <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
* operator interface as a common use case of the more generalized Trigger objects. This is a simple
* wrapper around Trigger with the method names renamed to fit the Button object use.
*/
@SuppressWarnings("PMD.TooManyMethods")
public abstract class Button extends Trigger {
/**
* Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
* overridden).
*/
public Button() {
}
/**
* Creates a new button with the given condition determining whether it is pressed.
*
* @param isPressed returns whether or not the trigger should be active
*/
public Button(BooleanSupplier isPressed) {
super(isPressed);
}
/**
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command, boolean interruptible) {
whenActive(command, interruptible);
return this;
}
/**
* Starts the given command whenever the button is newly pressed. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command) {
whenActive(command);
return this;
}
/**
* Runs the given runnable whenever the button is newly pressed.
*
* @param toRun the runnable to run
* @return this button, so calls can be chained
*/
public Button whenPressed(final Runnable toRun) {
whenActive(toRun);
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
* be canceled when the button is released.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whileHeld(final Command command, boolean interruptible) {
whileActiveContinuous(command, interruptible);
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
* be canceled when the button is released. The command is set to be interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whileHeld(final Command command) {
whileActiveContinuous(command);
return this;
}
/**
* Constantly runs the given runnable while the button is held.
*
* @param toRun the runnable to run
* @return this button, so calls can be chained
*/
public Button whileHeld(final Runnable toRun) {
whileActiveContinuous(toRun);
return this;
}
/**
* Starts the given command when the button is first pressed, and cancels it when it is released,
* but does not start it again if it ends or is otherwise interrupted.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command, boolean interruptible) {
whileActiveOnce(command, interruptible);
return this;
}
/**
* Starts the given command when the button is first pressed, and cancels it when it is released,
* but does not start it again if it ends or is otherwise interrupted. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command) {
whileActiveOnce(command, true);
return this;
}
/**
* Starts the command when the button is released.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this button, so calls can be chained
*/
public Button whenReleased(final Command command, boolean interruptible) {
whenInactive(command, interruptible);
return this;
}
/**
* Starts the command when the button is released. The command is set to be interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenReleased(final Command command) {
whenInactive(command);
return this;
}
/**
* Runs the given runnable when the button is released.
*
* @param toRun the runnable to run
* @return this button, so calls can be chained
*/
public Button whenReleased(final Runnable toRun) {
whenInactive(toRun);
return this;
}
/**
* Toggles the command whenever the button is pressed (on then off then on).
*
* @param command the command to start
* @param interruptible whether the command is interruptible
*/
public Button toggleWhenPressed(final Command command, boolean interruptible) {
toggleWhenActive(command, interruptible);
return this;
}
/**
* Toggles the command whenever the button is pressed (on then off then on). The command is set
* to be interruptible.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button toggleWhenPressed(final Command command) {
toggleWhenActive(command);
return this;
}
/**
* Cancels the command when the button is pressed.
*
* @param command the command to start
* @return this button, so calls can be chained
*/
public Button cancelWhenPressed(final Command command) {
cancelWhenActive(command);
return this;
}
}

47
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
View File

@@ -1,47 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command.button;
/**
* This class is intended to be used within a program. The programmer can manually set its value.
* Also includes a setting for whether or not it should invert its value.
*/
public class InternalButton extends Button {
private boolean m_pressed;
private boolean m_inverted;
/**
* Creates an InternalButton that is not inverted.
*/
public InternalButton() {
this(false);
}
/**
* Creates an InternalButton which is inverted depending on the input.
*
* @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
* when set to false.
*/
public InternalButton(boolean inverted) {
m_pressed = m_inverted = inverted;
}
public void setInverted(boolean inverted) {
m_inverted = inverted;
}
public void setPressed(boolean pressed) {
m_pressed = pressed;
}
@Override
public boolean get() {
return m_pressed ^ m_inverted;
}
}

44
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
View File

@@ -1,44 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command.button;
import edu.wpi.first.wpilibj.GenericHID;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A {@link Button} that gets its state from a {@link GenericHID}.
*/
public class JoystickButton extends Button {
private final GenericHID m_joystick;
private final int m_buttonNumber;
/**
* Creates a joystick button for triggering commands.
*
* @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick,
* etc)
* @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
*/
public JoystickButton(GenericHID joystick, int buttonNumber) {
requireNonNullParam(joystick, "joystick", "JoystickButton");
m_joystick = joystick;
m_buttonNumber = buttonNumber;
}
/**
* Gets the value of the joystick button.
*
* @return The value of the joystick button
*/
@Override
public boolean get() {
return m_joystick.getRawButton(m_buttonNumber);
}
}

57
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
View File

@@ -1,57 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command.button;
import edu.wpi.first.wpilibj.GenericHID;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* A {@link Button} that gets its state from a POV on a {@link GenericHID}.
*/
public class POVButton extends Button {
private final GenericHID m_joystick;
private final int m_angle;
private final int m_povNumber;
/**
* Creates a POV button for triggering commands.
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle in degrees (e.g. 90, 270)
* @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
*/
public POVButton(GenericHID joystick, int angle, int povNumber) {
requireNonNullParam(joystick, "joystick", "POVButton");
m_joystick = joystick;
m_angle = angle;
m_povNumber = povNumber;
}
/**
* Creates a POV button for triggering commands.
* By default, acts on POV 0
*
* @param joystick The GenericHID object that has the POV
* @param angle The desired angle (e.g. 90, 270)
*/
public POVButton(GenericHID joystick, int angle) {
this(joystick, angle, 0);
}
/**
* Checks whether the current value of the POV is the target angle.
*
* @return Whether the value of the POV matches the target angle
*/
@Override
public boolean get() {
return m_joystick.getPOV(m_povNumber) == m_angle;
}
}

351
wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
View File

@@ -1,351 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj2.command.button;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* This class provides an easy way to link commands to inputs.
*
* <p>It is very easy to link a button to a command. For instance, you could link the trigger
* button of a joystick to a "score" command.
*
* <p>It is encouraged that teams write a subclass of Trigger if they want to have something
* unusual (for instance, if they want to react to the user holding a button while the robot is
* reading a certain sensor input). For this, they only have to write the {@link Trigger#get()}
* method to get the full functionality of the Trigger class.
*/
@SuppressWarnings("PMD.TooManyMethods")
public class Trigger {
private final BooleanSupplier m_isActive;
/**
* Creates a new trigger with the given condition determining whether it is active.
*
* @param isActive returns whether or not the trigger should be active
*/
public Trigger(BooleanSupplier isActive) {
m_isActive = isActive;
}
/**
* Creates a new trigger that is always inactive. Useful only as a no-arg constructor for
* subclasses that will be overriding {@link Trigger#get()} anyway.
*/
public Trigger() {
m_isActive = () -> false;
}
/**
* Returns whether or not the trigger is active.
*
* <p>This method will be called repeatedly a command is linked to the Trigger.
*
* @return whether or not the trigger condition is active.
*/
public boolean get() {
return m_isActive.getAsBoolean();
}
/**
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whenActive");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
command.schedule(interruptible);
}
m_pressedLast = pressed;
}
});
return this;
}
/**
* Starts the given command whenever the trigger just becomes active. The command is set to be
* interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command) {
return whenActive(command, true);
}
/**
* Runs the given runnable whenever the trigger just becomes active.
*
* @param toRun the runnable to run
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Runnable toRun) {
return whenActive(new InstantCommand(toRun));
}
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* will be canceled when the trigger becomes inactive.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whileActiveContinuous");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
if (pressed) {
command.schedule(interruptible);
} else if (m_pressedLast) {
command.cancel();
}
m_pressedLast = pressed;
}
});
return this;
}
/**
* Constantly starts the given command while the button is held.
*
* {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
* will be canceled when the trigger becomes inactive. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command) {
return whileActiveContinuous(command, true);
}
/**
* Constantly runs the given runnable while the button is held.
*
* @param toRun the runnable to run
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Runnable toRun) {
return whileActiveContinuous(new InstantCommand(toRun));
}
/**
* Starts the given command when the trigger initially becomes active, and ends it when it becomes
* inactive, but does not re-start it in-between.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveOnce(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whileActiveOnce");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
command.schedule(interruptible);
} else if (m_pressedLast && !pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
});
return this;
}
/**
* Starts the given command when the trigger initially becomes active, and ends it when it becomes
* inactive, but does not re-start it in-between. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveOnce(final Command command) {
return whileActiveOnce(command, true);
}
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "whenInactive");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
if (m_pressedLast && !pressed) {
command.schedule(interruptible);
}
m_pressedLast = pressed;
}
});
return this;
}
/**
* Starts the command when the trigger becomes inactive. The command is set to be interruptible.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command) {
return whenInactive(command, true);
}
/**
* Runs the given runnable when the trigger becomes inactive.
*
* @param toRun the runnable to run
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Runnable toRun) {
return whenInactive(new InstantCommand(toRun));
}
/**
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
* @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
public Trigger toggleWhenActive(final Command command, boolean interruptible) {
requireNonNullParam(command, "command", "toggleWhenActive");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
if (command.isScheduled()) {
command.cancel();
} else {
command.schedule(interruptible);
}
}
m_pressedLast = pressed;
}
});
return this;
}
/**
* Toggles a command when the trigger becomes active. The command is set to be interruptible.
*
* @param command the command to toggle
* @return this trigger, so calls can be chained
*/
public Trigger toggleWhenActive(final Command command) {
return toggleWhenActive(command, true);
}
/**
* Cancels a command when the trigger becomes active.
*
* @param command the command to cancel
* @return this trigger, so calls can be chained
*/
public Trigger cancelWhenActive(final Command command) {
requireNonNullParam(command, "command", "cancelWhenActive");
CommandScheduler.getInstance().addButton(new Runnable() {
private boolean m_pressedLast = get();
@Override
public void run() {
boolean pressed = get();
if (!m_pressedLast && pressed) {
command.cancel();
}
m_pressedLast = pressed;
}
});
return this;
}
/**
* Composes this trigger with another trigger, returning a new trigger that is active when both
* triggers are active.
*
* @param trigger the trigger to compose with
* @return the trigger that is active when both triggers are active
*/
public Trigger and(Trigger trigger) {
return new Trigger(() -> get() && trigger.get());
}
/**
* Composes this trigger with another trigger, returning a new trigger that is active when either
* trigger is active.
*
* @param trigger the trigger to compose with
* @return the trigger that is active when either trigger is active
*/
public Trigger or(Trigger trigger) {
return new Trigger(() -> get() || trigger.get());
}
/**
* Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
* negation of this trigger.
*
* @return the negated trigger
*/
public Trigger negate() {
return new Trigger(() -> !get());
}
}