Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21: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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java Normal file
View File

@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* 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.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java Normal file
View File

@@ -0,0 +1,47 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java Normal file
View File

@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* 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.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java Normal file
View File

@@ -0,0 +1,32 @@
/*----------------------------------------------------------------------------*/
/* 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.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java Normal file
View File

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* 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.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java Normal file
View File

@@ -0,0 +1,185 @@
/*----------------------------------------------------------------------------*/
/* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java Normal file
View File

@@ -0,0 +1,670 @@
/*----------------------------------------------------------------------------*/
/* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java Normal file
View File

@@ -0,0 +1,422 @@
/*----------------------------------------------------------------------------*/
/* 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.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java Normal file
View File

@@ -0,0 +1,180 @@
/*----------------------------------------------------------------------------*/
/* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java Normal file
View File

@@ -0,0 +1,36 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java Normal file
View File

@@ -0,0 +1,110 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java Normal file
View File

@@ -0,0 +1,63 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java Normal file
View File

@@ -0,0 +1,284 @@
/*----------------------------------------------------------------------------*/
/* 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 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java Normal file
View File

@@ -0,0 +1,287 @@
/*----------------------------------------------------------------------------*/
/* 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 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java Normal file
View File

@@ -0,0 +1,360 @@
/*----------------------------------------------------------------------------*/
/* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java Normal file
View File

@@ -0,0 +1,48 @@
/*----------------------------------------------------------------------------*/
/* 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.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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java Normal file
View File

@@ -0,0 +1,255 @@
/*----------------------------------------------------------------------------*/
/* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java Normal file
View File

@@ -0,0 +1,62 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java Normal file
View File

@@ -0,0 +1,23 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* 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
wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java Normal file
View File

@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* 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 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;
}
}