Squashed 'third_party/allwpilib_2019/' changes from 936627bd9..a3f7420da
e20d96ea4 Use __has_include for WPILib.h (#2164)
a76d006a0 Update native-utils to 2020.7.2 (#2161)
24c031d69 Increase SPI auto byte count to allow 32 bytes to be sent (#2163)
6b4eecf5f Add hidden functions to get the SPI system and SPI DMA (#2162)
ccdd0fbdb Add TrapezoidProfile external PID examples (#2131)
5c6b8a0f4 Replace std::is_pod_v with std::is_standard_layout_v (#2159)
67d2fed68 Add DutyCycleEncoder channel constructor (#2158)
d8f11eb14 Hardcode channels for LSB weight (#2153)
b2ae75acd Add way to disable "no extensions found" message (#2134)
4f951789f Build testbench tests online inorder to improve speed (#2144)
005c4c5be Try catch around task dependencies to fix loading in editor (#2156)
34f6b3f4c Fix C++ RamseteCommand param doxygen (#2157)
f7a93713f Fix up templated TrapezoidProfile classes (#2151)
8c2ff94d7 Rename MathUtils to MathUtil for consistency with other util classes (#2155)
d003ec2dc Update to 2020v9 image (#2154)
8e7cc3fe7 Add user-friendly SimDeviceSim wrappers (#2150)
6c8f6cf47 Fix bug in cubic and quintic hermetic spline generation (#2139)
e37ecd33a Sim GUI: Add support for LED displays (#2138)
57c5523d6 Fix documentation in RamseteCommand (#2149)
7b9c6ebc2 Fix wpiutilJNIShared linkage typo in wpilibj (#2143)
9a515c80f Template C++ LinearFilter to work with unit types (#2142)
5b73c17f2 Remove encoder velocities methods in DifferentialDriveOdometry (#2147)
b8c102426 Fix PS3Eye VID and PID (#2146)
2622c6c29 Add default values for b and zeta in RamseteController (#2145)
f66ae5999 Add HSV helpers to AddressableLED (#2135)
5e97c81d8 Add MedianFilter class for moving-window median (#2136)
f79b7a058 Remove unnecessary constructor arg for LinearFilter's circular buffers (#2140)
e49494830 Replace Jenkins with Azure agent (#1914)
b67d049ac Check status of PDP CAN reads (#2126)
70102a60b SendableRegistry.foreachLiveWindow: Prevent concurrent modification (#2129)
6dcd2b0e2 Improve various subsystem APIs (#2130)
ce3973435 HAL_CAN_ReceiveMessage: return MessageNotFound if no callbacks registered (#2133)
3fcfc8ea7 Fix double disable segfaulting interrupts (#2132)
6ceafe3cd Fix class reference for logger (#2123)
b058dcf64 Catch exceptions generated by OpenCV in cscore JNI (#2118)
0b9307fdf Remove unused parts of .styleguide files (#2119)
39be812b2 Fix C++ ArmFeedforward (#2120)
21e957ee4 Add DifferentialDrive voltage constraint (#2075)
e0bc97f66 Add ProfiledPIDSubsystem example (#2076)
3df44c874 Remove Rotation2d.h wpi/math include (#2117)
a58dbec8a Add holonomic follower examples (#2052)
9a8067465 Fix incomplete .styleguide (#2113)
ffa4b907c Fix C++ floating point literal formatting (#2114)
3d1ca856b Fix missing typename and return type (#2115)
5f85457a9 Add usage reporting for AddressableLED (#2108)
4ebae1712 Enforce leading/trailing zeros in Java numeric constants (#2105)
fa85fbfc1 Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
f62e23f1a Add Doxygen for new HAL interfaces (#2110)
5443fdabc Directly construct PWM port from HAL, avoid wpilib PWM object (#2106)
c0e36df9d Standardize on PWMVictorSPX in examples (#2104)
8c4d9f541 Add TrapezoidProfileSubsystem (#2077)
45201d15f Add encoder distance overload to DifferentialDriveOdometry (#2096)
845aba33f Make feedforward classes constexpr (#2103)
500c43fb8 Add examples for DMA, DutyCycle, DutyCycleEncoder and AddressableLED (#2100)
589162811 Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)
b37b68daa Add JRE deployment to MyRobot Deploy (#2099)
0e83c65d2 Fix small logic error in ParallelDeadlineGroup (#2095)
6f6c6da9f Updates to addressable LED (#2098)
1894219ef Fix devmain package in commands (#2097)
77a9949bb Add AddressableLED simulation GUI support
a4c9e4ec2 Add AddressableLED simulation support
8ed205907 Add AddressableLED (#2092)
59507b12d Bump to 2020 v7 image (#2086)
5d39bf806 Make halsimgui::DrawLEDs() values parameter const (#2088)
841ef91c0 Use gyro angle instead of robot angle for odometry (#2081)
1b66ead49 Use standard constant for pi instead of 3.14 (#2084)
db2c3dddd Use DMA Global Number for DMA Index (#2085)
82b2170fe Add DMA support to HAL and WPILibC (#2080)
8280b7e3a Add DutyCycleEncoder and AnalogEncoder (#2040)
551096006 Use kNumSystems for DutyCycle count in Ports (#2083)
df1065218 Remove release configs of deploy in MyRobot (#2082)
bf5388393 Add deploy options to myRobot (#2079)
b7bc1ea74 Update to 2020v6 image (#2078)
708009cd2 Update to gradle 6.0 (#2074)
3cce61b89 Add SmartDashboard::GetEntry function in C++ (#2064)
565e1f3e7 Fix spelling in MecanumDriveOdometry docs (#2072)
1853f7b6b Add timing window to simulation GUI
c5a049712 Add simulation pause/resume/step support
f5446c740 Add Notifier HALSIM access
3e049e02f Add name to HAL Notifier
2da64d15f Make usage reporting enums type match (#2069)
f04d95e50 Make FRCUsageReporting.h C-compatible (#2070)
d748c67a5 Generate docs for command libraries and fix doclint enable (#2071)
55a7f2b4a Add template for old command-based style (#2031)
486fa9c69 Add XboxController examples for arcade and tank drive (#2058)
e3dd1c5d7 Fix small bug in SplineHelper (#2061)
7dc7c71b5 Add feedforward components (#2045)
5f33d6af1 Fix ProfiledPIDSubsystem parameter name (#2017)
94843adb8 Standardize documentation of Speed Controllers bounds (#2043)
9bcff37b9 Add HAL specific version of wpi_setError (#2055)
326aecc9a Add error message for CAN Write Overrun (#2062)
00228678d Add requirements param to more Command APIs (#2059)
ff39a96ce Make DigitalOutput a DigitalSource (#2054)
5ccad2e8a Fix frc2::Timer returning incorrect timestamp values (#2057)
629e95776 Add VendorDeps JSON files for command libraries (#2048)
6858a57f7 Make notifier command tests a lot more lenient (#2056)
0ebe32823 Fix MyRobotStatic accidentally linking to shared command libs (#2046)
384d00f9e Fix various duty cycle bugs (#2047)
1f6850adf Add CAN Manufacturer for Studica (#2050)
7508aada9 Add ability to end startCompetition() main loop (#2032)
f5b4be16d Old C++ Command: Make GetName et al public (#2042)
e6f5c93ab Clean up new C++ commands (#2027)
39f46ceab Don't allow rising and falling values to be read from AnalogTrigger (#2039)
d93aa2b6b Add missing lock in FRCDriverStation (#2034)
114ddaf81 Fix duplicate encoders in examples (#2033)
f22d0961e Sim GUI: Add duty cycle support
3262c2bad Sim GUI: Use new multi-channel PDP getter function
96d40192a Revert accidental change to MyRobot.cpp (#2029)
ed30d5d40 Add JSON support for Trajectories (#2025)
2b6811edd Fix null pointer dereference in C++ CommandScheduler (#2023)
1d695a166 Add FPGA Duty Cycle support (#1987)
509819d83 Split the two command implementations into separate libraries (#2012)
2ad15cae1 Add multi PDP getter and sim PCM/PDP multi arg functions (#2014)
931b8ceef Add new usage reporting types from 2020v5 (#2026)
0b3821eba Change files with CRLF line endings to LF (#2022)
6f159d142 Add way to atomically check for new data, and wait otherwise (#2015)
a769f1f22 Fix bug in RamseteCommand (using degrees instead of radians) (#2020)
c5186d815 Clean up PIDCommand (#2010)
9ebd23d61 Add setVoltage method to SpeedController (#1997)
f6e311ef8 Fix SplineHelper bug (#2018)
f33bd9f05 Fix NPE in RamseteCommand (#2019)
1c1e0c9a6 Add HAL_SetAllSolenoids to sim (#2004)
ea9bb651a Remove accidental OpenCV link from wpilibc shared library (#2013)
cc0742518 Change command decorators to return implementation (#2007)
16b34cce2 Remove IterativeRobot templates (#2011)
669127e49 Update intellisense to work with Beta 2020 code (#2008)
9dc30797e Fix usage reporting indices (#2009)
f6b844ea3 Move HAL Interrupt struct to anonymous namespace (#2003)
a72f80991 Add extern C to DigitalGlitchFilterJNI (#2002)
916596cb0 Fix invalid examples json, add validator (#2001)
5509a8e96 Use constexpr for all example constants
0be6b6475 Use constexpr for DifferentialDriveKinematics
Change-Id: I1416646cbab487ad8021830215766d8ec7f24ddc
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: a3f7420dab7a104c27a0c3bf0872c999c98fd9a9
diff --git a/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java b/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
new file mode 100644
index 0000000..5a5f6a9
--- /dev/null
+++ b/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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.commands;
+
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpiutil.RuntimeDetector;
+
+public final class DevMain {
+ /**
+ * Main entry point.
+ */
+ public static void main(String[] args) {
+ System.out.println("Hello World!");
+ System.out.println(RuntimeDetector.getPlatformPath());
+ System.out.println(NetworkTablesJNI.now());
+ System.out.println(HALUtil.getHALRuntimeType());
+ }
+
+ private DevMain() {
+ }
+}
diff --git a/wpilibOldCommands/src/dev/native/cpp/main.cpp b/wpilibOldCommands/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..5312a1d
--- /dev/null
+++ b/wpilibOldCommands/src/dev/native/cpp/main.cpp
@@ -0,0 +1,8 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+int main() {}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
new file mode 100644
index 0000000..dcd4401
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
new file mode 100644
index 0000000..c2a6eb1
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
@@ -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;
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
new file mode 100644
index 0000000..8d2b20e
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
new file mode 100644
index 0000000..3176977
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
new file mode 100644
index 0000000..39c891d
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
@@ -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;
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
new file mode 100644
index 0000000..f051776
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
new file mode 100644
index 0000000..da135ca
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
new file mode 100644
index 0000000..525e681
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
@@ -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;
+ }
+ }
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
new file mode 100644
index 0000000..2dc6f66
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
@@ -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();
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
new file mode 100644
index 0000000..1053f55
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
new file mode 100644
index 0000000..f255e48
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
@@ -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();
+ }
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
new file mode 100644
index 0000000..78da002
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
@@ -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;
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
new file mode 100644
index 0000000..8b60254
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
@@ -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");
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
new file mode 100644
index 0000000..1f30f38
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
@@ -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();
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
new file mode 100644
index 0000000..b8d7fed
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
new file mode 100644
index 0000000..e25a5a7
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
@@ -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);
+ }
+ }
+ });
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
new file mode 100644
index 0000000..6aac6d7
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
@@ -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();
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
new file mode 100644
index 0000000..bd8c658
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
@@ -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();
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
new file mode 100644
index 0000000..3c2858f
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
new file mode 100644
index 0000000..386bb0e
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
@@ -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();
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
new file mode 100644
index 0000000..0e1762a
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
@@ -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);
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
new file mode 100644
index 0000000..118da6a
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
@@ -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();
+ }
+}
diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
new file mode 100644
index 0000000..dadcab5
--- /dev/null
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
@@ -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;
+ }
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
new file mode 100644
index 0000000..57c86bd
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/Button.h"
+
+using namespace frc;
+
+void Button::WhenPressed(Command* command) { WhenActive(command); }
+
+void Button::WhileHeld(Command* command) { WhileActive(command); }
+
+void Button::WhenReleased(Command* command) { WhenInactive(command); }
+
+void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
+
+void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
new file mode 100644
index 0000000..f79c487
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ButtonScheduler.h"
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
+ : m_pressedLast(last), m_button(button), m_command(orders) {}
+
+void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
new file mode 100644
index 0000000..b0f4433
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/CancelButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void CancelButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (!m_pressedLast && pressed) {
+ m_command->Cancel();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
new file mode 100644
index 0000000..9abf8bc
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/HeldButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void HeldButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (pressed) {
+ m_command->Start();
+ } else if (m_pressedLast && !pressed) {
+ m_command->Cancel();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
new file mode 100644
index 0000000..6ff7971
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/InternalButton.h"
+
+using namespace frc;
+
+InternalButton::InternalButton(bool inverted)
+ : m_pressed(inverted), m_inverted(inverted) {}
+
+void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
+
+void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
+
+bool InternalButton::Get() { return m_pressed ^ m_inverted; }
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
new file mode 100644
index 0000000..c1d5a29
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/JoystickButton.h"
+
+using namespace frc;
+
+JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
+ : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
+
+bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
new file mode 100644
index 0000000..8bca356
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/NetworkButton.h"
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+
+using namespace frc;
+
+NetworkButton::NetworkButton(const wpi::Twine& tableName,
+ const wpi::Twine& field)
+ : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
+ field) {}
+
+NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+ const wpi::Twine& field)
+ : m_entry(table->GetEntry(field)) {}
+
+bool NetworkButton::Get() {
+ return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
new file mode 100644
index 0000000..73e847a
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/POVButton.h"
+
+using namespace frc;
+
+POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
+ : m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
+
+bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
new file mode 100644
index 0000000..4a470f5
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/PressedButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void PressedButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (!m_pressedLast && pressed) {
+ m_command->Start();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
new file mode 100644
index 0000000..671c13e
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ReleasedButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void ReleasedButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (m_pressedLast && !pressed) {
+ m_command->Start();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
new file mode 100644
index 0000000..cefccb5
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ToggleButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void ToggleButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (!m_pressedLast && pressed) {
+ if (m_command->IsRunning()) {
+ m_command->Cancel();
+ } else {
+ m_command->Start();
+ }
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
new file mode 100644
index 0000000..f215083
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/Button.h"
+#include "frc/buttons/CancelButtonScheduler.h"
+#include "frc/buttons/HeldButtonScheduler.h"
+#include "frc/buttons/PressedButtonScheduler.h"
+#include "frc/buttons/ReleasedButtonScheduler.h"
+#include "frc/buttons/ToggleButtonScheduler.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
+
+Trigger& Trigger::operator=(const Trigger& rhs) {
+ SendableHelper::operator=(rhs);
+ m_sendablePressed = false;
+ return *this;
+}
+
+Trigger::Trigger(Trigger&& rhs)
+ : SendableHelper(std::move(rhs)),
+ m_sendablePressed(rhs.m_sendablePressed.load()) {
+ rhs.m_sendablePressed = false;
+}
+
+Trigger& Trigger::operator=(Trigger&& rhs) {
+ SendableHelper::operator=(std::move(rhs));
+ m_sendablePressed = rhs.m_sendablePressed.load();
+ rhs.m_sendablePressed = false;
+ return *this;
+}
+
+bool Trigger::Grab() { return Get() || m_sendablePressed; }
+
+void Trigger::WhenActive(Command* command) {
+ auto pbs = new PressedButtonScheduler(Grab(), this, command);
+ pbs->Start();
+}
+
+void Trigger::WhileActive(Command* command) {
+ auto hbs = new HeldButtonScheduler(Grab(), this, command);
+ hbs->Start();
+}
+
+void Trigger::WhenInactive(Command* command) {
+ auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
+ rbs->Start();
+}
+
+void Trigger::CancelWhenActive(Command* command) {
+ auto cbs = new CancelButtonScheduler(Grab(), this, command);
+ cbs->Start();
+}
+
+void Trigger::ToggleWhenActive(Command* command) {
+ auto tbs = new ToggleButtonScheduler(Grab(), this, command);
+ tbs->Start();
+}
+
+void Trigger::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Button");
+ builder.SetSafeState([=]() { m_sendablePressed = false; });
+ builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
+ [=](bool value) { m_sendablePressed = value; });
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
new file mode 100644
index 0000000..a7154c0
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
@@ -0,0 +1,264 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Command.h"
+
+#include <typeinfo>
+
+#include "frc/RobotState.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/commands/CommandGroup.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+int Command::m_commandCounter = 0;
+
+Command::Command() : Command("", -1.0) {}
+
+Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
+
+Command::Command(double timeout) : Command("", timeout) {}
+
+Command::Command(Subsystem& subsystem) : Command("", -1.0) {
+ Requires(&subsystem);
+}
+
+Command::Command(const wpi::Twine& name, double timeout) {
+ // We use -1.0 to indicate no timeout.
+ if (timeout < 0.0 && timeout != -1.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+
+ m_timeout = timeout;
+
+ // If name contains an empty string
+ if (name.isTriviallyEmpty() ||
+ (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
+ SendableRegistry::GetInstance().Add(
+ this, "Command_" + wpi::Twine(typeid(*this).name()));
+ } else {
+ SendableRegistry::GetInstance().Add(this, name);
+ }
+}
+
+Command::Command(const wpi::Twine& name, Subsystem& subsystem)
+ : Command(name, -1.0) {
+ Requires(&subsystem);
+}
+
+Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
+ Requires(&subsystem);
+}
+
+Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
+ : Command(name, timeout) {
+ Requires(&subsystem);
+}
+
+double Command::TimeSinceInitialized() const {
+ if (m_startTime < 0.0)
+ return 0.0;
+ else
+ return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+void Command::Requires(Subsystem* subsystem) {
+ if (!AssertUnlocked("Can not add new requirement to command")) return;
+
+ if (subsystem != nullptr)
+ m_requirements.insert(subsystem);
+ else
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+}
+
+void Command::Start() {
+ LockChanges();
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not start a command that is part of a command group");
+
+ m_completed = false;
+ Scheduler::GetInstance()->AddCommand(this);
+}
+
+bool Command::Run() {
+ if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
+ Cancel();
+
+ if (IsCanceled()) return false;
+
+ if (!m_initialized) {
+ m_initialized = true;
+ StartTiming();
+ _Initialize();
+ Initialize();
+ }
+ _Execute();
+ Execute();
+ return !IsFinished();
+}
+
+void Command::Cancel() {
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not cancel a command that is part of a command group");
+
+ _Cancel();
+}
+
+bool Command::IsRunning() const { return m_running; }
+
+bool Command::IsInitialized() const { return m_initialized; }
+
+bool Command::IsCompleted() const { return m_completed; }
+
+bool Command::IsCanceled() const { return m_canceled; }
+
+bool Command::IsInterruptible() const { return m_interruptible; }
+
+void Command::SetInterruptible(bool interruptible) {
+ m_interruptible = interruptible;
+}
+
+bool Command::DoesRequire(Subsystem* system) const {
+ return m_requirements.count(system) > 0;
+}
+
+const Command::SubsystemSet& Command::GetRequirements() const {
+ return m_requirements;
+}
+
+CommandGroup* Command::GetGroup() const { return m_parent; }
+
+void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+
+bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+
+int Command::GetID() const { return m_commandID; }
+
+void Command::SetTimeout(double timeout) {
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ else
+ m_timeout = timeout;
+}
+
+bool Command::IsTimedOut() const {
+ return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+bool Command::AssertUnlocked(const std::string& message) {
+ if (m_locked) {
+ std::string buf =
+ message + " after being started or being added to a command group";
+ wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
+ return false;
+ }
+ return true;
+}
+
+void Command::SetParent(CommandGroup* parent) {
+ if (parent == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "parent");
+ } else if (m_parent != nullptr) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Can not give command to a command group after "
+ "already being put in a command group");
+ } else {
+ LockChanges();
+ m_parent = parent;
+ }
+}
+
+bool Command::IsParented() const { return m_parent != nullptr; }
+
+void Command::ClearRequirements() { m_requirements.clear(); }
+
+void Command::Initialize() {}
+
+void Command::Execute() {}
+
+void Command::End() {}
+
+void Command::Interrupted() { End(); }
+
+void Command::_Initialize() { m_completed = false; }
+
+void Command::_Interrupted() { m_completed = true; }
+
+void Command::_Execute() {}
+
+void Command::_End() { m_completed = true; }
+
+void Command::_Cancel() {
+ if (IsRunning()) m_canceled = true;
+}
+
+void Command::LockChanges() { m_locked = true; }
+
+void Command::Removed() {
+ if (m_initialized) {
+ if (IsCanceled()) {
+ Interrupted();
+ _Interrupted();
+ } else {
+ End();
+ _End();
+ }
+ }
+ m_initialized = false;
+ m_canceled = false;
+ m_running = false;
+ m_completed = true;
+}
+
+void Command::StartRunning() {
+ m_running = true;
+ m_startTime = -1;
+ m_completed = false;
+}
+
+void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+
+std::string Command::GetName() const {
+ return SendableRegistry::GetInstance().GetName(this);
+}
+
+void Command::SetName(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string Command::GetSubsystem() const {
+ return SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void Command::SetSubsystem(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void Command::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Command");
+ builder.AddStringProperty(
+ ".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
+ nullptr);
+ builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
+ [=](bool value) {
+ if (value) {
+ if (!IsRunning()) Start();
+ } else {
+ if (IsRunning()) Cancel();
+ }
+ });
+ builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
+ nullptr);
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
new file mode 100644
index 0000000..f64ba67
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/CommandGroup.h"
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
+
+void CommandGroup::AddSequential(Command* command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddSequential(Command* command, double timeout) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+ if (timeout < 0.0) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ return;
+ }
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
+ timeout);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command, double timeout) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+ if (timeout < 0.0) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ return;
+ }
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
+ timeout);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+bool CommandGroup::IsInterruptible() const {
+ if (!Command::IsInterruptible()) return false;
+
+ if (m_currentCommandIndex != -1 &&
+ static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+ Command* cmd = m_commands[m_currentCommandIndex].m_command;
+ if (!cmd->IsInterruptible()) return false;
+ }
+
+ for (const auto& child : m_children) {
+ if (!child->m_command->IsInterruptible()) return false;
+ }
+
+ return true;
+}
+
+int CommandGroup::GetSize() const { return m_children.size(); }
+
+void CommandGroup::Initialize() {}
+
+void CommandGroup::Execute() {}
+
+bool CommandGroup::IsFinished() {
+ return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
+ m_children.empty();
+}
+
+void CommandGroup::End() {}
+
+void CommandGroup::Interrupted() {}
+
+void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+
+void CommandGroup::_Execute() {
+ CommandGroupEntry* entry;
+ Command* cmd = nullptr;
+ bool firstRun = false;
+
+ if (m_currentCommandIndex == -1) {
+ firstRun = true;
+ m_currentCommandIndex = 0;
+ }
+
+ // While there are still commands in this group to run
+ while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+ // If a command is prepared to run
+ if (cmd != nullptr) {
+ // If command timed out, cancel it so it's removed from the Scheduler
+ if (entry->IsTimedOut()) cmd->_Cancel();
+
+ // If command finished or was cancelled, remove it from Scheduler
+ if (cmd->Run()) {
+ break;
+ } else {
+ cmd->Removed();
+
+ // Advance to next command in group
+ m_currentCommandIndex++;
+ firstRun = true;
+ cmd = nullptr;
+ continue;
+ }
+ }
+
+ entry = &m_commands[m_currentCommandIndex];
+ cmd = nullptr;
+
+ switch (entry->m_state) {
+ case CommandGroupEntry::kSequence_InSequence:
+ cmd = entry->m_command;
+ if (firstRun) {
+ cmd->StartRunning();
+ CancelConflicts(cmd);
+ firstRun = false;
+ }
+ break;
+
+ case CommandGroupEntry::kSequence_BranchPeer:
+ // Start executing a parallel command and advance to next entry in group
+ m_currentCommandIndex++;
+ entry->m_command->Start();
+ break;
+
+ case CommandGroupEntry::kSequence_BranchChild:
+ m_currentCommandIndex++;
+
+ /* Causes scheduler to skip children of current command which require
+ * the same subsystems as it
+ */
+ CancelConflicts(entry->m_command);
+ entry->m_command->StartRunning();
+
+ // Add current command entry to list of children of this group
+ m_children.push_back(entry);
+ break;
+ }
+ }
+
+ // Run Children
+ for (auto& entry : m_children) {
+ auto child = entry->m_command;
+ if (entry->IsTimedOut()) {
+ child->_Cancel();
+ }
+
+ // If child finished or was cancelled, set it to nullptr. nullptr entries
+ // are removed later.
+ if (!child->Run()) {
+ child->Removed();
+ entry = nullptr;
+ }
+ }
+
+ m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
+ m_children.end());
+}
+
+void CommandGroup::_End() {
+ // Theoretically, we don't have to check this, but we do if teams override the
+ // IsFinished method
+ if (m_currentCommandIndex != -1 &&
+ static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+ Command* cmd = m_commands[m_currentCommandIndex].m_command;
+ cmd->_Cancel();
+ cmd->Removed();
+ }
+
+ for (auto& child : m_children) {
+ Command* cmd = child->m_command;
+ cmd->_Cancel();
+ cmd->Removed();
+ }
+ m_children.clear();
+}
+
+void CommandGroup::_Interrupted() { _End(); }
+
+void CommandGroup::CancelConflicts(Command* command) {
+ for (auto childIter = m_children.begin(); childIter != m_children.end();) {
+ Command* child = (*childIter)->m_command;
+ bool erased = false;
+
+ for (auto&& requirement : command->GetRequirements()) {
+ if (child->DoesRequire(requirement)) {
+ child->_Cancel();
+ child->Removed();
+ childIter = m_children.erase(childIter);
+ erased = true;
+ break;
+ }
+ }
+ if (!erased) childIter++;
+ }
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
new file mode 100644
index 0000000..3441743
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/CommandGroupEntry.h"
+
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
+ double timeout)
+ : m_timeout(timeout), m_command(command), m_state(state) {}
+
+bool CommandGroupEntry::IsTimedOut() const {
+ if (m_timeout < 0.0) return false;
+ double time = m_command->TimeSinceInitialized();
+ if (time == 0.0) return false;
+ return time >= m_timeout;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
new file mode 100644
index 0000000..1c75c65
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/ConditionalCommand.h"
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
+ if (onTrue != nullptr) {
+ for (auto requirement : onTrue->GetRequirements())
+ command.Requires(requirement);
+ }
+ if (onFalse != nullptr) {
+ for (auto requirement : onFalse->GetRequirements())
+ command.Requires(requirement);
+ }
+}
+
+ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ RequireAll(*this, onTrue, onFalse);
+}
+
+ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+ Command* onFalse)
+ : Command(name) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ RequireAll(*this, onTrue, onFalse);
+}
+
+void ConditionalCommand::_Initialize() {
+ if (Condition()) {
+ m_chosenCommand = m_onTrue;
+ } else {
+ m_chosenCommand = m_onFalse;
+ }
+
+ if (m_chosenCommand != nullptr) {
+ // This is a hack to make cancelling the chosen command inside a
+ // CommandGroup work properly
+ m_chosenCommand->ClearRequirements();
+
+ m_chosenCommand->Start();
+ }
+ Command::_Initialize();
+}
+
+void ConditionalCommand::_Cancel() {
+ if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+ m_chosenCommand->Cancel();
+ }
+
+ Command::_Cancel();
+}
+
+bool ConditionalCommand::IsFinished() {
+ if (m_chosenCommand != nullptr) {
+ return m_chosenCommand->IsCompleted();
+ } else {
+ return true;
+ }
+}
+
+void ConditionalCommand::_Interrupted() {
+ if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+ m_chosenCommand->Cancel();
+ }
+
+ Command::_Interrupted();
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
new file mode 100644
index 0000000..445c5e0
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/InstantCommand.h"
+
+using namespace frc;
+
+InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
+
+InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
+
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
+ : Command(name, subsystem) {}
+
+InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
+
+InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
+ : InstantCommand(subsystem) {
+ m_func = func;
+}
+
+InstantCommand::InstantCommand(const wpi::Twine& name,
+ std::function<void()> func)
+ : InstantCommand(name) {
+ m_func = func;
+}
+
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+ std::function<void()> func)
+ : InstantCommand(name, subsystem) {
+ m_func = func;
+}
+
+void InstantCommand::_Initialize() {
+ Command::_Initialize();
+ if (m_func) {
+ m_func();
+ }
+}
+
+bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
new file mode 100644
index 0000000..90a05f4
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PIDCommand.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double f, double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double f, double period, Subsystem& subsystem)
+ : Command(name, subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
+ Subsystem& subsystem)
+ : Command(subsystem) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ Subsystem& subsystem)
+ : Command(name, subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double period, Subsystem& subsystem)
+ : Command(name, subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period,
+ Subsystem& subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+void PIDCommand::_Initialize() { m_controller->Enable(); }
+
+void PIDCommand::_End() { m_controller->Disable(); }
+
+void PIDCommand::_Interrupted() { _End(); }
+
+void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDCommand::PIDGet() { return ReturnPIDInput(); }
+
+std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
+ return m_controller;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
+
+double PIDCommand::GetPosition() { return ReturnPIDInput(); }
+
+void PIDCommand::InitSendable(SendableBuilder& builder) {
+ m_controller->InitSendable(builder);
+ Command::InitSendable(builder);
+ builder.SetSmartDashboardType("PIDCommand");
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp b/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..4d35943
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PIDSubsystem.h"
+
+#include "frc/PIDController.h"
+
+using namespace frc;
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+ double f)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+ double f, double period)
+ : Subsystem(name) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
+ double period)
+ : Subsystem("PIDSubsystem") {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+ AddChild("PIDController", m_controller);
+}
+
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
+ m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
+ m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
+ m_controller->SetAbsoluteTolerance(absValue);
+}
+
+void PIDSubsystem::SetPercentTolerance(double percent) {
+ m_controller->SetPercentTolerance(percent);
+}
+
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+ return m_controller;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
new file mode 100644
index 0000000..888e8a1
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PrintCommand.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+ : InstantCommand("Print \"" + message + wpi::Twine('"')) {
+ m_message = message.str();
+}
+
+void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
new file mode 100644
index 0000000..6944e41
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Scheduler.h"
+
+#include <algorithm>
+#include <set>
+#include <string>
+#include <vector>
+
+#include <hal/FRCUsageReporting.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/mutex.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/buttons/ButtonScheduler.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Subsystem.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+struct Scheduler::Impl {
+ void Remove(Command* command);
+ void ProcessCommandAddition(Command* command);
+
+ using SubsystemSet = std::set<Subsystem*>;
+ SubsystemSet subsystems;
+ wpi::mutex buttonsMutex;
+ using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
+ ButtonVector buttons;
+ using CommandVector = std::vector<Command*>;
+ wpi::mutex additionsMutex;
+ CommandVector additions;
+ using CommandSet = std::set<Command*>;
+ CommandSet commands;
+ bool adding = false;
+ bool enabled = true;
+ std::vector<std::string> commandsBuf;
+ std::vector<double> idsBuf;
+ bool runningCommandsChanged = false;
+};
+
+Scheduler* Scheduler::GetInstance() {
+ static Scheduler instance;
+ return &instance;
+}
+
+void Scheduler::AddCommand(Command* command) {
+ std::scoped_lock lock(m_impl->additionsMutex);
+ if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
+ m_impl->additions.end())
+ return;
+ m_impl->additions.push_back(command);
+}
+
+void Scheduler::AddButton(ButtonScheduler* button) {
+ std::scoped_lock lock(m_impl->buttonsMutex);
+ m_impl->buttons.emplace_back(button);
+}
+
+void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
+ if (subsystem == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+ return;
+ }
+ m_impl->subsystems.insert(subsystem);
+}
+
+void Scheduler::Run() {
+ // Get button input (going backwards preserves button priority)
+ {
+ if (!m_impl->enabled) return;
+
+ std::scoped_lock lock(m_impl->buttonsMutex);
+ for (auto& button : m_impl->buttons) {
+ button->Execute();
+ }
+ }
+
+ // Call every subsystem's periodic method
+ for (auto& subsystem : m_impl->subsystems) {
+ subsystem->Periodic();
+ }
+
+ m_impl->runningCommandsChanged = false;
+
+ // Loop through the commands
+ for (auto cmdIter = m_impl->commands.begin();
+ cmdIter != m_impl->commands.end();) {
+ Command* command = *cmdIter;
+ // Increment before potentially removing to keep the iterator valid
+ ++cmdIter;
+ if (!command->Run()) {
+ Remove(command);
+ m_impl->runningCommandsChanged = true;
+ }
+ }
+
+ // Add the new things
+ {
+ std::scoped_lock lock(m_impl->additionsMutex);
+ for (auto& addition : m_impl->additions) {
+ // Check to make sure no adding during adding
+ if (m_impl->adding) {
+ wpi_setWPIErrorWithContext(IncompatibleState,
+ "Can not start command from cancel method");
+ } else {
+ m_impl->ProcessCommandAddition(addition);
+ }
+ }
+ m_impl->additions.clear();
+ }
+
+ // Add in the defaults
+ for (auto& subsystem : m_impl->subsystems) {
+ if (subsystem->GetCurrentCommand() == nullptr) {
+ if (m_impl->adding) {
+ wpi_setWPIErrorWithContext(IncompatibleState,
+ "Can not start command from cancel method");
+ } else {
+ m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
+ }
+ }
+ subsystem->ConfirmCommand();
+ }
+}
+
+void Scheduler::Remove(Command* command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+
+ m_impl->Remove(command);
+}
+
+void Scheduler::RemoveAll() {
+ while (m_impl->commands.size() > 0) {
+ Remove(*m_impl->commands.begin());
+ }
+}
+
+void Scheduler::ResetAll() {
+ RemoveAll();
+ m_impl->subsystems.clear();
+ m_impl->buttons.clear();
+ m_impl->additions.clear();
+ m_impl->commands.clear();
+}
+
+void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
+
+void Scheduler::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Scheduler");
+ auto namesEntry = builder.GetEntry("Names");
+ auto idsEntry = builder.GetEntry("Ids");
+ auto cancelEntry = builder.GetEntry("Cancel");
+ builder.SetUpdateTable([=]() {
+ // Get the list of possible commands to cancel
+ auto new_toCancel = cancelEntry.GetValue();
+ wpi::ArrayRef<double> toCancel;
+ if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
+
+ // Cancel commands whose cancel buttons were pressed on the SmartDashboard
+ if (!toCancel.empty()) {
+ for (auto& command : m_impl->commands) {
+ for (const auto& cancelled : toCancel) {
+ if (command->GetID() == cancelled) {
+ command->Cancel();
+ }
+ }
+ }
+ nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
+ }
+
+ // Set the running commands
+ if (m_impl->runningCommandsChanged) {
+ m_impl->commandsBuf.resize(0);
+ m_impl->idsBuf.resize(0);
+ auto& registry = SendableRegistry::GetInstance();
+ for (const auto& command : m_impl->commands) {
+ m_impl->commandsBuf.emplace_back(registry.GetName(command));
+ m_impl->idsBuf.emplace_back(command->GetID());
+ }
+ nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
+ nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
+ }
+ });
+}
+
+Scheduler::Scheduler() : m_impl(new Impl) {
+ HAL_Report(HALUsageReporting::kResourceType_Command,
+ HALUsageReporting::kCommand_Scheduler);
+ SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+Scheduler::~Scheduler() {}
+
+void Scheduler::Impl::Remove(Command* command) {
+ if (!commands.erase(command)) return;
+
+ for (auto&& requirement : command->GetRequirements()) {
+ requirement->SetCurrentCommand(nullptr);
+ }
+
+ command->Removed();
+}
+
+void Scheduler::Impl::ProcessCommandAddition(Command* command) {
+ if (command == nullptr) return;
+
+ // Only add if not already in
+ auto found = commands.find(command);
+ if (found == commands.end()) {
+ // Check that the requirements can be had
+ const auto& requirements = command->GetRequirements();
+ for (const auto& requirement : requirements) {
+ if (requirement->GetCurrentCommand() != nullptr &&
+ !requirement->GetCurrentCommand()->IsInterruptible())
+ return;
+ }
+
+ // Give it the requirements
+ adding = true;
+ for (auto&& requirement : requirements) {
+ if (requirement->GetCurrentCommand() != nullptr) {
+ requirement->GetCurrentCommand()->Cancel();
+ Remove(requirement->GetCurrentCommand());
+ }
+ requirement->SetCurrentCommand(command);
+ }
+ adding = false;
+
+ commands.insert(command);
+
+ command->StartRunning();
+ runningCommandsChanged = true;
+ }
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
new file mode 100644
index 0000000..97ddfb9
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/StartCommand.h"
+
+using namespace frc;
+
+StartCommand::StartCommand(Command* commandToStart)
+ : InstantCommand("StartCommand") {
+ m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
new file mode 100644
index 0000000..6e665ea
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Subsystem.h"
+
+#include "frc/WPIErrors.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Subsystem::Subsystem(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().AddLW(this, name, name);
+ Scheduler::GetInstance()->RegisterSubsystem(this);
+}
+
+void Subsystem::SetDefaultCommand(Command* command) {
+ if (command == nullptr) {
+ m_defaultCommand = nullptr;
+ } else {
+ const auto& reqs = command->GetRequirements();
+ if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse, "A default command must require the subsystem");
+ return;
+ }
+
+ m_defaultCommand = command;
+ }
+}
+
+Command* Subsystem::GetDefaultCommand() {
+ if (!m_initializedDefaultCommand) {
+ m_initializedDefaultCommand = true;
+ InitDefaultCommand();
+ }
+ return m_defaultCommand;
+}
+
+wpi::StringRef Subsystem::GetDefaultCommandName() {
+ Command* defaultCommand = GetDefaultCommand();
+ if (defaultCommand) {
+ return SendableRegistry::GetInstance().GetName(defaultCommand);
+ } else {
+ return wpi::StringRef();
+ }
+}
+
+void Subsystem::SetCurrentCommand(Command* command) {
+ m_currentCommand = command;
+ m_currentCommandChanged = true;
+}
+
+Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+
+wpi::StringRef Subsystem::GetCurrentCommandName() const {
+ Command* currentCommand = GetCurrentCommand();
+ if (currentCommand) {
+ return SendableRegistry::GetInstance().GetName(currentCommand);
+ } else {
+ return wpi::StringRef();
+ }
+}
+
+void Subsystem::Periodic() {}
+
+void Subsystem::InitDefaultCommand() {}
+
+std::string Subsystem::GetName() const {
+ return SendableRegistry::GetInstance().GetName(this);
+}
+
+void Subsystem::SetName(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string Subsystem::GetSubsystem() const {
+ return SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void Subsystem::SetSubsystem(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name,
+ std::shared_ptr<Sendable> child) {
+ AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
+ AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddLW(&child, registry.GetSubsystem(this), name);
+ registry.AddChild(this, &child);
+}
+
+void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable& child) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.SetSubsystem(&child, registry.GetSubsystem(this));
+ registry.EnableLiveWindow(&child);
+ registry.AddChild(this, &child);
+}
+
+void Subsystem::ConfirmCommand() {
+ if (m_currentCommandChanged) m_currentCommandChanged = false;
+}
+
+void Subsystem::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Subsystem");
+
+ builder.AddBooleanProperty(
+ ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
+ builder.AddStringProperty(".default",
+ [=]() { return GetDefaultCommandName(); }, nullptr);
+
+ builder.AddBooleanProperty(
+ ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
+ builder.AddStringProperty(".command",
+ [=]() { return GetCurrentCommandName(); }, nullptr);
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
new file mode 100644
index 0000000..f5113b2
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/TimedCommand.h"
+
+using namespace frc;
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
+ : Command(name, timeout) {}
+
+TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
+ Subsystem& subsystem)
+ : Command(name, timeout, subsystem) {}
+
+TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
+ : Command(timeout, subsystem) {}
+
+bool TimedCommand::IsFinished() { return IsTimedOut(); }
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
new file mode 100644
index 0000000..d326193
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitCommand.h"
+
+using namespace frc;
+
+WaitCommand::WaitCommand(double timeout)
+ : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
+
+WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
+ : TimedCommand(name, timeout) {}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
new file mode 100644
index 0000000..374c11c
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitForChildren.h"
+
+#include "frc/commands/CommandGroup.h"
+
+using namespace frc;
+
+WaitForChildren::WaitForChildren(double timeout)
+ : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
+ : Command(name, timeout) {}
+
+bool WaitForChildren::IsFinished() {
+ return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
+}
diff --git a/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
new file mode 100644
index 0000000..07ff00b
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitUntilCommand.h"
+
+#include "frc/Timer.h"
+
+using namespace frc;
+
+WaitUntilCommand::WaitUntilCommand(double time)
+ : Command("WaitUntilCommand", time) {
+ m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
+ : Command(name, time) {
+ m_time = time;
+}
+
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h b/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
new file mode 100644
index 0000000..c0830bf
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/Trigger.h"
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * 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.
+ *
+ * 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.
+ */
+class Button : public Trigger {
+ public:
+ Button() = default;
+ Button(Button&&) = default;
+ Button& operator=(Button&&) = default;
+
+ /**
+ * Specifies the command to run when a button is first pressed.
+ *
+ * @param command The pointer to the command to run
+ */
+ virtual void WhenPressed(Command* command);
+
+ /**
+ * Specifies the command to be scheduled while the button is pressed.
+ *
+ * The command will be scheduled repeatedly while the button is pressed and
+ * will be canceled when the button is released.
+ *
+ * @param command The pointer to the command to run
+ */
+ virtual void WhileHeld(Command* command);
+
+ /**
+ * Specifies the command to run when the button is released.
+ *
+ * The command will be scheduled a single time.
+ *
+ * @param command The pointer to the command to run
+ */
+ virtual void WhenReleased(Command* command);
+
+ /**
+ * Cancels the specificed command when the button is pressed.
+ *
+ * @param command The command to be canceled
+ */
+ virtual void CancelWhenPressed(Command* command);
+
+ /**
+ * Toggle the specified command when the button is pressed.
+ *
+ * @param command The command to be toggled
+ */
+ virtual void ToggleWhenPressed(Command* command);
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
new file mode 100644
index 0000000..960fbc8
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class ButtonScheduler {
+ public:
+ ButtonScheduler(bool last, Trigger* button, Command* orders);
+ virtual ~ButtonScheduler() = default;
+
+ ButtonScheduler(ButtonScheduler&&) = default;
+ ButtonScheduler& operator=(ButtonScheduler&&) = default;
+
+ virtual void Execute() = 0;
+ void Start();
+
+ protected:
+ bool m_pressedLast;
+ Trigger* m_button;
+ Command* m_command;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
new file mode 100644
index 0000000..9131fc0
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class CancelButtonScheduler : public ButtonScheduler {
+ public:
+ CancelButtonScheduler(bool last, Trigger* button, Command* orders);
+ virtual ~CancelButtonScheduler() = default;
+
+ CancelButtonScheduler(CancelButtonScheduler&&) = default;
+ CancelButtonScheduler& operator=(CancelButtonScheduler&&) = default;
+
+ virtual void Execute();
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
new file mode 100644
index 0000000..2b51ce6
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class HeldButtonScheduler : public ButtonScheduler {
+ public:
+ HeldButtonScheduler(bool last, Trigger* button, Command* orders);
+ virtual ~HeldButtonScheduler() = default;
+
+ HeldButtonScheduler(HeldButtonScheduler&&) = default;
+ HeldButtonScheduler& operator=(HeldButtonScheduler&&) = default;
+
+ virtual void Execute();
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
new file mode 100644
index 0000000..ed4dc56
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/Button.h"
+
+namespace frc {
+
+class InternalButton : public Button {
+ public:
+ InternalButton() = default;
+ explicit InternalButton(bool inverted);
+ virtual ~InternalButton() = default;
+
+ InternalButton(InternalButton&&) = default;
+ InternalButton& operator=(InternalButton&&) = default;
+
+ void SetInverted(bool inverted);
+ void SetPressed(bool pressed);
+
+ virtual bool Get();
+
+ private:
+ bool m_pressed = false;
+ bool m_inverted = false;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
new file mode 100644
index 0000000..e0e2d4c
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/GenericHID.h"
+#include "frc/buttons/Button.h"
+
+namespace frc {
+
+class JoystickButton : public Button {
+ public:
+ JoystickButton(GenericHID* joystick, int buttonNumber);
+ virtual ~JoystickButton() = default;
+
+ JoystickButton(JoystickButton&&) = default;
+ JoystickButton& operator=(JoystickButton&&) = default;
+
+ virtual bool Get();
+
+ private:
+ GenericHID* m_joystick;
+ int m_buttonNumber;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
new file mode 100644
index 0000000..f16821b
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/Twine.h>
+
+#include "frc/buttons/Button.h"
+
+namespace frc {
+
+class NetworkButton : public Button {
+ public:
+ NetworkButton(const wpi::Twine& tableName, const wpi::Twine& field);
+ NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+ const wpi::Twine& field);
+ virtual ~NetworkButton() = default;
+
+ NetworkButton(NetworkButton&&) = default;
+ NetworkButton& operator=(NetworkButton&&) = default;
+
+ virtual bool Get();
+
+ private:
+ nt::NetworkTableEntry m_entry;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h b/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
new file mode 100644
index 0000000..bd73984
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/GenericHID.h"
+#include "frc/buttons/Button.h"
+
+namespace frc {
+class POVButton : public Button {
+ public:
+ /**
+ * 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 GenericHID#GetPOV)
+ */
+ POVButton(GenericHID& joystick, int angle, int povNumber = 0);
+ virtual ~POVButton() = default;
+
+ POVButton(POVButton&&) = default;
+ POVButton& operator=(POVButton&&) = default;
+
+ bool Get() override;
+
+ private:
+ GenericHID* m_joystick;
+ int m_angle;
+ int m_povNumber;
+};
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
new file mode 100644
index 0000000..29c6a95
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class PressedButtonScheduler : public ButtonScheduler {
+ public:
+ PressedButtonScheduler(bool last, Trigger* button, Command* orders);
+ virtual ~PressedButtonScheduler() = default;
+
+ PressedButtonScheduler(PressedButtonScheduler&&) = default;
+ PressedButtonScheduler& operator=(PressedButtonScheduler&&) = default;
+
+ virtual void Execute();
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
new file mode 100644
index 0000000..a567a7b
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class ReleasedButtonScheduler : public ButtonScheduler {
+ public:
+ ReleasedButtonScheduler(bool last, Trigger* button, Command* orders);
+ virtual ~ReleasedButtonScheduler() = default;
+
+ ReleasedButtonScheduler(ReleasedButtonScheduler&&) = default;
+ ReleasedButtonScheduler& operator=(ReleasedButtonScheduler&&) = default;
+
+ virtual void Execute();
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h b/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
new file mode 100644
index 0000000..406d131
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/buttons/ButtonScheduler.h"
+
+namespace frc {
+
+class Trigger;
+class Command;
+
+class ToggleButtonScheduler : public ButtonScheduler {
+ public:
+ ToggleButtonScheduler(bool last, Trigger* button, Command* orders);
+ virtual ~ToggleButtonScheduler() = default;
+
+ ToggleButtonScheduler(ToggleButtonScheduler&&) = default;
+ ToggleButtonScheduler& operator=(ToggleButtonScheduler&&) = default;
+
+ virtual void Execute();
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h b/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
new file mode 100644
index 0000000..56700e9
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class Command;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * It is very easy to link a polled input to a command. For instance, you could
+ * link the trigger button of a joystick to a "score" command or an encoder
+ * reaching a particular value.
+ *
+ * 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.
+ */
+class Trigger : public Sendable, public SendableHelper<Trigger> {
+ public:
+ Trigger() = default;
+ ~Trigger() override = default;
+
+ Trigger(const Trigger& rhs);
+ Trigger& operator=(const Trigger& rhs);
+ Trigger(Trigger&& rhs);
+ Trigger& operator=(Trigger&& rhs);
+
+ bool Grab();
+ virtual bool Get() = 0;
+ void WhenActive(Command* command);
+ void WhileActive(Command* command);
+ void WhenInactive(Command* command);
+ void CancelWhenActive(Command* command);
+ void ToggleWhenActive(Command* command);
+
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ std::atomic_bool m_sendablePressed{false};
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/Command.h b/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
new file mode 100644
index 0000000..241490a
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
@@ -0,0 +1,493 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/SmallPtrSet.h>
+#include <wpi/Twine.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/commands/Subsystem.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class CommandGroup;
+
+/**
+ * The Command class is at the very core of the entire command framework.
+ *
+ * Every command can be started with a call to Start(). Once a command is
+ * started it will call Initialize(), and then will repeatedly call Execute()
+ * until the IsFinished() returns true. Once it does,End() will be called.
+ *
+ * However, if at any point while it is running Cancel() is called, then the
+ * command will be stopped and Interrupted() will be called.
+ *
+ * If a command uses a Subsystem, then it should specify that it does so by
+ * calling the Requires() method in its constructor. Note that a Command may
+ * have multiple requirements, and Requires() should be called for each one.
+ *
+ * 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 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 CommandGroup
+ * @see Subsystem
+ */
+class Command : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Command> {
+ friend class CommandGroup;
+ friend class Scheduler;
+
+ public:
+ /**
+ * Creates a new command.
+ *
+ * The name of this command will be default.
+ */
+ Command();
+
+ /**
+ * Creates a new command with the given name and no timeout.
+ *
+ * @param name the name for this command
+ */
+ explicit Command(const wpi::Twine& name);
+
+ /**
+ * Creates a new command with the given timeout and a default name.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see IsTimedOut()
+ */
+ explicit Command(double timeout);
+
+ /**
+ * Creates a new command with the given timeout and a default name.
+ *
+ * @param subsystem the subsystem that the command requires
+ */
+ explicit Command(Subsystem& 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"
+ * @see IsTimedOut()
+ */
+ Command(const wpi::Twine& name, double timeout);
+
+ /**
+ * Creates a new command with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param subsystem the subsystem that the command requires
+ */
+ Command(const wpi::Twine& name, Subsystem& subsystem);
+
+ /**
+ * Creates a new command with the given name and timeout.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ * @param subsystem the subsystem that the command requires @see IsTimedOut()
+ */
+ Command(double timeout, Subsystem& 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"
+ * @param subsystem the subsystem that the command requires @see IsTimedOut()
+ */
+ Command(const wpi::Twine& name, double timeout, Subsystem& subsystem);
+
+ ~Command() override = default;
+
+ Command(Command&&) = default;
+ Command& operator=(Command&&) = default;
+
+ /**
+ * 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).
+ */
+ double TimeSinceInitialized() const;
+
+ /**
+ * This method specifies that the given Subsystem is used by this command.
+ *
+ * This method is crucial to the functioning of the Command System in general.
+ *
+ * Note that the recommended way to call this method is in the constructor.
+ *
+ * @param subsystem The Subsystem required
+ * @see Subsystem
+ */
+ void Requires(Subsystem* s);
+
+ /**
+ * Starts up the command. Gets the command ready to start.
+ *
+ * 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.
+ */
+ void Start();
+
+ /**
+ * The run method is used internally to actually run the commands.
+ *
+ * @return Whether or not the command should stay within the Scheduler.
+ */
+ bool Run();
+
+ /**
+ * This will cancel the current command.
+ *
+ * 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.
+ *
+ * A command can not be canceled if it is a part of a command group, you must
+ * cancel the command group instead.
+ */
+ void Cancel();
+
+ /**
+ * 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 Interrupted().
+ *
+ * @return whether or not the command is running
+ */
+ bool IsRunning() const;
+
+ /**
+ * Returns whether or not the command has been initialized.
+ *
+ * @return whether or not the command has been initialized.
+ */
+ bool IsInitialized() const;
+
+ /**
+ * Returns whether or not the command has completed running.
+ *
+ * @return whether or not the command has completed running.
+ */
+ bool IsCompleted() const;
+
+ /**
+ * Returns whether or not this has been canceled.
+ *
+ * @return whether or not this has been canceled
+ */
+ bool IsCanceled() const;
+
+ /**
+ * Returns whether or not this command can be interrupted.
+ *
+ * @return whether or not this command can be interrupted
+ */
+ bool IsInterruptible() const;
+
+ /**
+ * Sets whether or not this command can be interrupted.
+ *
+ * @param interruptible whether or not this command can be interrupted
+ */
+ void SetInterruptible(bool interruptible);
+
+ /**
+ * Checks if the command requires the given Subsystem.
+ *
+ * @param system the system
+ * @return whether or not the subsystem is required (false if given nullptr)
+ */
+ bool DoesRequire(Subsystem* subsystem) const;
+
+ using SubsystemSet = wpi::SmallPtrSetImpl<Subsystem*>;
+
+ /**
+ * Returns the requirements (as an std::set of Subsystem pointers) of this
+ * command.
+ *
+ * @return The requirements (as an std::set of Subsystem pointers) of this
+ * command
+ */
+ const SubsystemSet& GetRequirements() const;
+
+ /**
+ * Returns the CommandGroup that this command is a part of.
+ *
+ * Will return null if this Command is not in a group.
+ *
+ * @return The CommandGroup that this command is a part of (or null if not in
+ * group)
+ */
+ CommandGroup* GetGroup() const;
+
+ /**
+ * Sets whether or not this Command should run when the robot is disabled.
+ *
+ * By default a command will not run when the robot is disabled, and will in
+ * fact be canceled.
+ *
+ * @param run Whether this command should run when the robot is disabled.
+ */
+ void SetRunWhenDisabled(bool run);
+
+ /**
+ * Returns whether or not this Command will run when the robot is disabled, or
+ * if it will cancel itself.
+ *
+ * @return Whether this Command will run when the robot is disabled, or if it
+ * will cancel itself.
+ */
+ bool WillRunWhenDisabled() const;
+
+ /**
+ * Get the ID (sequence number) for this command.
+ *
+ * The ID is a unique sequence number that is incremented for each command.
+ *
+ * @return The ID of this command
+ */
+ int GetID() const;
+
+ protected:
+ /**
+ * Sets the timeout of this command.
+ *
+ * @param timeout the timeout (in seconds)
+ * @see IsTimedOut()
+ */
+ void SetTimeout(double timeout);
+
+ /**
+ * Returns whether or not the 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
+ */
+ bool IsTimedOut() const;
+
+ /**
+ * If changes are locked, then this will generate a CommandIllegalUse error.
+ *
+ * @param message The message to report on error (it is appended by a default
+ * message)
+ * @return True if assert passed, false if assert failed.
+ */
+ bool AssertUnlocked(const std::string& message);
+
+ /**
+ * Sets the parent of this command. No actual change is made to the group.
+ *
+ * @param parent the parent
+ */
+ void SetParent(CommandGroup* parent);
+
+ /**
+ * Returns whether the command has a parent.
+ *
+ * @param True if the command has a parent.
+ */
+ bool IsParented() const;
+
+ /**
+ * Clears list of subsystem requirements.
+ *
+ * This is only used by ConditionalCommand so cancelling the chosen command
+ * works properly in CommandGroup.
+ */
+ void ClearRequirements();
+
+ /**
+ * The initialize method is called the first time this Command is run after
+ * being started.
+ */
+ virtual void Initialize();
+
+ /**
+ * The execute method is called repeatedly until this Command either finishes
+ * or is canceled.
+ */
+ virtual void Execute();
+
+ /**
+ * Returns whether this command is finished.
+ *
+ * If it is, then the command will be removed and End() will be called.
+ *
+ * It may be useful for a team to reference the IsTimedOut() method for
+ * time-sensitive commands.
+ *
+ * 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 InstantCommand for this.
+ *
+ * @return Whether this command is finished.
+ * @see IsTimedOut()
+ */
+ virtual bool IsFinished() = 0;
+
+ /**
+ * 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.
+ */
+ virtual void End();
+
+ /**
+ * Called when the command ends because somebody called Cancel() or another
+ * command shared the same requirements as this one, and booted it out.
+ *
+ * This is where you may want to wrap up loose ends, like shutting off a motor
+ * that was being used in the command.
+ *
+ * Generally, it is useful to simply call the End() method within this method,
+ * as done here.
+ */
+ virtual void Interrupted();
+
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _Execute();
+ virtual void _End();
+
+ /**
+ * 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.
+ */
+ virtual void _Cancel();
+
+ friend class ConditionalCommand;
+
+ public:
+ /**
+ * Gets the name of this Command.
+ *
+ * @return Name
+ */
+ std::string GetName() const;
+
+ /**
+ * Sets the name of this Command.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the subsystem name of this Command.
+ *
+ * @return Subsystem name
+ */
+ std::string GetSubsystem() const;
+
+ /**
+ * Sets the subsystem name of this Command.
+ *
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(const wpi::Twine& subsystem);
+
+ private:
+ /**
+ * Prevents further changes from being made.
+ */
+ void LockChanges();
+
+ /**
+ * Called when the command has been removed.
+ *
+ * This will call Interrupted() or End().
+ */
+ void Removed();
+
+ /**
+ * This is used internally to mark that the command has been started.
+ *
+ * The lifecycle of a command is:
+ *
+ * StartRunning() is called. Run() is called (multiple times potentially).
+ * Removed() is called.
+ *
+ * It is very important that StartRunning() and Removed() be called in order
+ * or some assumptions of the code will be broken.
+ */
+ void StartRunning();
+
+ /**
+ * Called to indicate that the timer should start.
+ *
+ * This is called right before Initialize() is, inside the Run() method.
+ */
+ void StartTiming();
+
+ // The time since this command was initialized
+ double m_startTime = -1;
+
+ // The time (in seconds) before this command "times out" (-1 if no timeout)
+ double m_timeout;
+
+ // Whether or not this command has been initialized
+ bool m_initialized = false;
+
+ // The requirements (or null if no requirements)
+ wpi::SmallPtrSet<Subsystem*, 4> m_requirements;
+
+ // Whether or not it is running
+ bool m_running = false;
+
+ // Whether or not it is interruptible
+ bool m_interruptible = true;
+
+ // Whether or not it has been canceled
+ bool m_canceled = false;
+
+ // Whether or not it has been locked
+ bool m_locked = false;
+
+ // Whether this command should run when the robot is disabled
+ bool m_runWhenDisabled = false;
+
+ // The CommandGroup this is in
+ CommandGroup* m_parent = nullptr;
+
+ // Whether or not this command has completed running
+ bool m_completed = false;
+
+ int m_commandID = m_commandCounter++;
+ static int m_commandCounter;
+
+ public:
+ void InitSendable(SendableBuilder& builder) override;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
new file mode 100644
index 0000000..0275cb5
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+#include "frc/commands/CommandGroupEntry.h"
+
+namespace frc {
+
+/**
+ * A CommandGroup is a list of commands which are executed in sequence.
+ *
+ * Commands in a CommandGroup are added using the AddSequential() method and are
+ * called sequentially. CommandGroups are themselves Commands and can be given
+ * to other CommandGroups.
+ *
+ * CommandGroups will carry all of the requirements of their Command
+ * subcommands. Additional requirements can be specified by calling Requires()
+ * normally in the constructor.
+ *
+ * CommandGroups can also execute commands in parallel, simply by adding them
+ * using AddParallel().
+ *
+ * @see Command
+ * @see Subsystem
+ */
+class CommandGroup : public Command {
+ public:
+ CommandGroup() = default;
+
+ /**
+ * Creates a new CommandGroup with the given name.
+ *
+ * @param name The name for this command group
+ */
+ explicit CommandGroup(const wpi::Twine& name);
+
+ virtual ~CommandGroup() = default;
+
+ CommandGroup(CommandGroup&&) = default;
+ CommandGroup& operator=(CommandGroup&&) = default;
+
+ /**
+ * Adds a new Command to the group. The Command will be started after all the
+ * previously added Commands.
+ *
+ * Note that any requirements the given Command has will be added to the
+ * group. For this reason, a Command's requirements can not be changed after
+ * being added to a group.
+ *
+ * It is recommended that this method be called in the constructor.
+ *
+ * @param command The Command to be added
+ */
+ void AddSequential(Command* command);
+
+ /**
+ * Adds a new Command to the group with a given timeout. The Command will be
+ * started after all the previously added commands.
+ *
+ * Once the Command is started, it will be run until it finishes or the time
+ * expires, whichever is sooner. Note that the given Command will have no
+ * knowledge that it is on a timer.
+ *
+ * Note that any requirements the given Command has will be added to the
+ * group. For this reason, a Command's requirements can not be changed after
+ * being added to a group.
+ *
+ * It is recommended that this method be called in the constructor.
+ *
+ * @param command The Command to be added
+ * @param timeout The timeout (in seconds)
+ */
+ void AddSequential(Command* command, double timeout);
+
+ /**
+ * Adds a new child Command to the group. The Command will be started after
+ * all the previously added Commands.
+ *
+ * Instead of waiting for the child to finish, a CommandGroup will have it run
+ * at the same time as the subsequent Commands. The child will run until
+ * either it finishes, a new child with conflicting requirements is started,
+ * or the main sequence runs a Command with conflicting requirements. In the
+ * latter two cases, the child will be canceled even if it says it can't be
+ * interrupted.
+ *
+ * Note that any requirements the given Command has will be added to the
+ * group. For this reason, a Command's requirements can not be changed after
+ * being added to a group.
+ *
+ * It is recommended that this method be called in the constructor.
+ *
+ * @param command The command to be added
+ */
+ void AddParallel(Command* command);
+
+ /**
+ * Adds a new child Command to the group with the given timeout. The Command
+ * will be started after all the previously added Commands.
+ *
+ * Once the Command is started, it will run until it finishes, is interrupted,
+ * or the time expires, whichever is sooner. Note that the given Command will
+ * have no knowledge that it is on a timer.
+ *
+ * Instead of waiting for the child to finish, a CommandGroup will have it run
+ * at the same time as the subsequent 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 Command with
+ * conflicting requirements. In the latter two cases, the child will be
+ * canceled even if it says it can't be interrupted.
+ *
+ * Note that any requirements the given Command has will be added to the
+ * group. For this reason, a Command's requirements can not be changed after
+ * being added to a group.
+ *
+ * It is recommended that this method be called in the constructor.
+ *
+ * @param command The command to be added
+ * @param timeout The timeout (in seconds)
+ */
+ void AddParallel(Command* command, double timeout);
+
+ bool IsInterruptible() const;
+
+ int GetSize() const;
+
+ protected:
+ /**
+ * Can be overridden by teams.
+ */
+ virtual void Initialize();
+
+ /**
+ * Can be overridden by teams.
+ */
+ virtual void Execute();
+
+ /**
+ * Can be overridden by teams.
+ */
+ virtual bool IsFinished();
+
+ /**
+ * Can be overridden by teams.
+ */
+ virtual void End();
+
+ /**
+ * Can be overridden by teams.
+ */
+ virtual void Interrupted();
+
+ virtual void _Initialize();
+ virtual void _Execute();
+ virtual void _End();
+ virtual void _Interrupted();
+
+ private:
+ void CancelConflicts(Command* command);
+
+ // The commands in this group (stored in entries)
+ std::vector<CommandGroupEntry> m_commands;
+
+ // The active children in this group (stored in entries)
+ std::vector<CommandGroupEntry*> m_children;
+
+ // The current command, -1 signifies that none have been run
+ int m_currentCommandIndex = -1;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
new file mode 100644
index 0000000..d1d2264
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+class Command;
+
+class CommandGroupEntry {
+ public:
+ enum Sequence {
+ kSequence_InSequence,
+ kSequence_BranchPeer,
+ kSequence_BranchChild
+ };
+
+ CommandGroupEntry() = default;
+ CommandGroupEntry(Command* command, Sequence state, double timeout = -1.0);
+
+ CommandGroupEntry(CommandGroupEntry&&) = default;
+ CommandGroupEntry& operator=(CommandGroupEntry&&) = default;
+
+ bool IsTimedOut() const;
+
+ double m_timeout = -1.0;
+ Command* m_command = nullptr;
+ Sequence m_state = kSequence_InSequence;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
new file mode 100644
index 0000000..f5cd738
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+/**
+ * A ConditionalCommand is a Command that starts one of two commands.
+ *
+ * A ConditionalCommand uses the Condition method to determine whether it should
+ * run onTrue or onFalse.
+ *
+ * A ConditionalCommand adds the proper Command to the Scheduler during
+ * Initialize() and then IsFinished() will return true once that Command has
+ * finished executing.
+ *
+ * If no Command is specified for onFalse, the occurrence of that condition
+ * will be a no-op.
+ *
+ * A ConditionalCommand will require the superset of subsystems of the onTrue
+ * and onFalse commands.
+ *
+ * @see Command
+ * @see Scheduler
+ */
+class ConditionalCommand : public Command {
+ public:
+ /**
+ * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+ *
+ * @param onTrue The Command to execute if Condition() returns true
+ * @param onFalse The Command to execute if Condition() returns false
+ */
+ explicit ConditionalCommand(Command* onTrue, Command* onFalse = nullptr);
+
+ /**
+ * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
+ *
+ * @param name The name for this command group
+ * @param onTrue The Command to execute if Condition() returns true
+ * @param onFalse The Command to execute if Condition() returns false
+ */
+ ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+ Command* onFalse = nullptr);
+
+ virtual ~ConditionalCommand() = default;
+
+ ConditionalCommand(ConditionalCommand&&) = default;
+ ConditionalCommand& operator=(ConditionalCommand&&) = default;
+
+ protected:
+ /**
+ * 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.
+ */
+ virtual bool Condition() = 0;
+
+ void _Initialize() override;
+ void _Cancel() override;
+ bool IsFinished() override;
+ void _Interrupted() override;
+
+ private:
+ // The Command to execute if Condition() returns true
+ Command* m_onTrue;
+
+ // The Command to execute if Condition() returns false
+ Command* m_onFalse;
+
+ // Stores command chosen by condition
+ Command* m_chosenCommand = nullptr;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
new file mode 100644
index 0000000..3663b7f
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+#include "frc/commands/Subsystem.h"
+
+namespace frc {
+
+/**
+ * This command will execute once, then finish immediately afterward.
+ *
+ * Subclassing InstantCommand is shorthand for returning true from IsFinished().
+ */
+class InstantCommand : public Command {
+ public:
+ /**
+ * Creates a new InstantCommand with the given name.
+ *
+ * @param name The name for this command
+ */
+ explicit InstantCommand(const wpi::Twine& name);
+
+ /**
+ * Creates a new InstantCommand with the given requirement.
+ *
+ * @param subsystem The subsystem that the command requires
+ */
+ explicit InstantCommand(Subsystem& subsystem);
+
+ /**
+ * Creates a new InstantCommand with the given name.
+ *
+ * @param name The name for this command
+ * @param subsystem The subsystem that the command requires
+ */
+ InstantCommand(const wpi::Twine& name, Subsystem& subsystem);
+
+ /**
+ * Create a command that calls the given function when run.
+ *
+ * @param func The function to run when Initialize() is run.
+ */
+ explicit InstantCommand(std::function<void()> func);
+
+ /**
+ * Create a command that calls the given function when run.
+ *
+ * @param subsystem The subsystems that this command runs on.
+ * @param func The function to run when Initialize() is run.
+ */
+ InstantCommand(Subsystem& subsystem, std::function<void()> func);
+
+ /**
+ * Create a command that calls the given function when run.
+ *
+ * @param name The name of the command.
+ * @param func The function to run when Initialize() is run.
+ */
+ InstantCommand(const wpi::Twine& name, std::function<void()> func);
+
+ /**
+ * Create a command that calls the given function when run.
+ *
+ * @param name The name of the command.
+ * @param subsystem The subsystems that this command runs on.
+ * @param func The function to run when Initialize() is run.
+ */
+ InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+ std::function<void()> func);
+
+ InstantCommand() = default;
+ virtual ~InstantCommand() = default;
+
+ InstantCommand(InstantCommand&&) = default;
+ InstantCommand& operator=(InstantCommand&&) = default;
+
+ protected:
+ std::function<void()> m_func = nullptr;
+ void _Initialize() override;
+ bool IsFinished() override;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
new file mode 100644
index 0000000..d00ac9b
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Twine.h>
+
+#include "frc/PIDController.h"
+#include "frc/PIDOutput.h"
+#include "frc/PIDSource.h"
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+class PIDCommand : public Command, public PIDOutput, public PIDSource {
+ public:
+ PIDCommand(const wpi::Twine& name, double p, double i, double d);
+ PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double period);
+ PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+ double period);
+ PIDCommand(double p, double i, double d);
+ PIDCommand(double p, double i, double d, double period);
+ PIDCommand(double p, double i, double d, double f, double period);
+ PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ Subsystem& subsystem);
+ PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double period, Subsystem& subsystem);
+ PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+ double period, Subsystem& subsystem);
+ PIDCommand(double p, double i, double d, Subsystem& subsystem);
+ PIDCommand(double p, double i, double d, double period, Subsystem& subsystem);
+ PIDCommand(double p, double i, double d, double f, double period,
+ Subsystem& subsystem);
+ virtual ~PIDCommand() = default;
+
+ PIDCommand(PIDCommand&&) = default;
+ PIDCommand& operator=(PIDCommand&&) = default;
+
+ void SetSetpointRelative(double deltaSetpoint);
+
+ // PIDOutput interface
+ void PIDWrite(double output) override;
+
+ // PIDSource interface
+ double PIDGet() override;
+
+ protected:
+ std::shared_ptr<PIDController> GetPIDController() const;
+ void _Initialize() override;
+ void _Interrupted() override;
+ void _End() override;
+ void SetSetpoint(double setpoint);
+ double GetSetpoint() const;
+ double GetPosition();
+
+ virtual double ReturnPIDInput() = 0;
+ virtual void UsePIDOutput(double output) = 0;
+
+ private:
+ // The internal PIDController
+ std::shared_ptr<PIDController> m_controller;
+
+ public:
+ void InitSendable(SendableBuilder& builder) override;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h b/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
new file mode 100644
index 0000000..2afc05b
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
@@ -0,0 +1,236 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Twine.h>
+
+#include "frc/PIDController.h"
+#include "frc/PIDOutput.h"
+#include "frc/PIDSource.h"
+#include "frc/commands/Subsystem.h"
+
+namespace frc {
+
+/**
+ * This class is designed to handle the case where there is a Subsystem which
+ * uses a single PIDController almost constantly (for instance, an elevator
+ * which attempts to stay at a constant height).
+ *
+ * It provides some convenience methods to run an internal PIDController. It
+ * also allows access to the internal PIDController in order to give total
+ * control to the programmer.
+ */
+class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
+ public:
+ /**
+ * Instantiates a 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
+ */
+ PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
+
+ /**
+ * Instantiates a 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 feedforward value
+ */
+ PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
+
+ /**
+ * Instantiates a 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 feedfoward value
+ * @param period the time (in seconds) between calculations
+ */
+ PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f,
+ double period);
+
+ /**
+ * Instantiates a 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
+ */
+ PIDSubsystem(double p, double i, double d);
+
+ /**
+ * Instantiates a 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
+ * @param f the feedforward value
+ */
+ PIDSubsystem(double p, double i, double d, double f);
+
+ /**
+ * Instantiates a 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 feedforward value
+ * @param period the time (in seconds) between calculations
+ */
+ PIDSubsystem(double p, double i, double d, double f, double period);
+
+ ~PIDSubsystem() override = default;
+
+ PIDSubsystem(PIDSubsystem&&) = default;
+ PIDSubsystem& operator=(PIDSubsystem&&) = default;
+
+ /**
+ * Enables the internal PIDController.
+ */
+ void Enable();
+
+ /**
+ * Disables the internal PIDController.
+ */
+ void Disable();
+
+ // PIDOutput interface
+ void PIDWrite(double output) override;
+
+ // PIDSource interface
+
+ double PIDGet() override;
+
+ /**
+ * Sets the setpoint to the given value.
+ *
+ * If SetRange() was called, then the given setpoint will be trimmed to fit
+ * within the range.
+ *
+ * @param setpoint the new setpoint
+ */
+ void SetSetpoint(double setpoint);
+
+ /**
+ * Adds the given value to the setpoint.
+ *
+ * If SetRange() was used, then the bounds will still be honored by this
+ * method.
+ *
+ * @param deltaSetpoint the change in the setpoint
+ */
+ void SetSetpointRelative(double deltaSetpoint);
+
+ /**
+ * 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
+ */
+ void SetInputRange(double minimumInput, double 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
+ */
+ void SetOutputRange(double minimumOutput, double maximumOutput);
+
+ /**
+ * Return the current setpoint.
+ *
+ * @return The current setpoint
+ */
+ double GetSetpoint();
+
+ /**
+ * Returns the current position.
+ *
+ * @return the current position
+ */
+ double GetPosition();
+
+ /**
+ * Returns the current rate.
+ *
+ * @return the current rate
+ */
+ double GetRate();
+
+ /**
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param absValue absolute error which is tolerable
+ */
+ virtual void SetAbsoluteTolerance(double absValue);
+
+ /**
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget().
+ *
+ * @param percent percentage error which is tolerable
+ */
+ virtual void SetPercentTolerance(double percent);
+
+ /**
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance().
+ *
+ * This asssumes that the maximum and minimum input were set using SetInput().
+ * Use OnTarget() in the IsFinished() method of commands that use this
+ * subsystem.
+ *
+ * Currently this just reports on target as the actual value passes through
+ * the setpoint. Ideally it should be based on being within the tolerance for
+ * some period of time.
+ *
+ * @return True if the error is within the percentage tolerance of the input
+ * range
+ */
+ virtual bool OnTarget() const;
+
+ protected:
+ /**
+ * Returns the PIDController used by this PIDSubsystem.
+ *
+ * Use this if you would like to fine tune the PID loop.
+ *
+ * @return The PIDController used by this PIDSubsystem
+ */
+ std::shared_ptr<PIDController> GetPIDController();
+
+ virtual double ReturnPIDInput() = 0;
+ virtual void UsePIDOutput(double output) = 0;
+
+ private:
+ // The internal PIDController
+ std::shared_ptr<PIDController> m_controller;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
new file mode 100644
index 0000000..5e987d3
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/InstantCommand.h"
+
+namespace frc {
+
+class PrintCommand : public InstantCommand {
+ public:
+ explicit PrintCommand(const wpi::Twine& message);
+ virtual ~PrintCommand() = default;
+
+ PrintCommand(PrintCommand&&) = default;
+ PrintCommand& operator=(PrintCommand&&) = default;
+
+ protected:
+ virtual void Initialize();
+
+ private:
+ std::string m_message;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h b/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
new file mode 100644
index 0000000..d15ea39
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class ButtonScheduler;
+class Command;
+class Subsystem;
+
+class Scheduler : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Scheduler> {
+ public:
+ /**
+ * Returns the Scheduler, creating it if one does not exist.
+ *
+ * @return the Scheduler
+ */
+ static Scheduler* GetInstance();
+
+ /**
+ * Add a command to be scheduled later.
+ *
+ * In any pass through the scheduler, all commands are added to the additions
+ * list, then at the end of the pass, they are all scheduled.
+ *
+ * @param command The command to be scheduled
+ */
+ void AddCommand(Command* command);
+
+ void AddButton(ButtonScheduler* button);
+
+ /**
+ * Registers a Subsystem to this Scheduler, so that the Scheduler might know
+ * if a default Command needs to be run.
+ *
+ * All Subsystems should call this.
+ *
+ * @param system the system
+ */
+ void RegisterSubsystem(Subsystem* subsystem);
+
+ /**
+ * Runs a single iteration of the loop.
+ *
+ * This method should be called often in order to have a functioning
+ * 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>
+ */
+ void Run();
+
+ /**
+ * Removes the Command from the Scheduler.
+ *
+ * @param command the command to remove
+ */
+ void Remove(Command* command);
+
+ void RemoveAll();
+
+ /**
+ * Completely resets the scheduler. Undefined behavior if running.
+ */
+ void ResetAll();
+
+ void SetEnabled(bool enabled);
+
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ Scheduler();
+ ~Scheduler() override;
+
+ Scheduler(Scheduler&&) = default;
+ Scheduler& operator=(Scheduler&&) = default;
+
+ struct Impl;
+ std::unique_ptr<Impl> m_impl;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
new file mode 100644
index 0000000..5062f92
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/commands/InstantCommand.h"
+
+namespace frc {
+
+class StartCommand : public InstantCommand {
+ public:
+ explicit StartCommand(Command* commandToStart);
+ virtual ~StartCommand() = default;
+
+ StartCommand(StartCommand&&) = default;
+ StartCommand& operator=(StartCommand&&) = default;
+
+ protected:
+ virtual void Initialize();
+
+ private:
+ Command* m_commandToFork;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h b/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
new file mode 100644
index 0000000..2637dd0
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
@@ -0,0 +1,199 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class Command;
+
+class Subsystem : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Subsystem> {
+ friend class Scheduler;
+
+ public:
+ /**
+ * Creates a subsystem with the given name.
+ *
+ * @param name the name of the subsystem
+ */
+ explicit Subsystem(const wpi::Twine& name);
+
+ Subsystem(Subsystem&&) = default;
+ Subsystem& operator=(Subsystem&&) = default;
+
+ /**
+ * Sets the default command. If this is not called or is called with null,
+ * then there will be no default command for the subsystem.
+ *
+ * <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the
+ * subsystem is a singleton.
+ *
+ * @param command the default command (or null if there should be none)
+ */
+ void SetDefaultCommand(Command* command);
+
+ /**
+ * Returns the default command (or null if there is none).
+ *
+ * @return the default command
+ */
+ Command* GetDefaultCommand();
+
+ /**
+ * Returns the default command name, or empty string is there is none.
+ *
+ * @return the default command name
+ */
+ wpi::StringRef GetDefaultCommandName();
+
+ /**
+ * Sets the current command.
+ *
+ * @param command the new current command
+ */
+ void SetCurrentCommand(Command* command);
+
+ /**
+ * Returns the command which currently claims this subsystem.
+ *
+ * @return the command which currently claims this subsystem
+ */
+ Command* GetCurrentCommand() const;
+
+ /**
+ * Returns the current command name, or empty string if no current command.
+ *
+ * @return the current command name
+ */
+ wpi::StringRef GetCurrentCommandName() const;
+
+ /**
+ * When the run method of the scheduler is called this method will be called.
+ */
+ virtual void Periodic();
+
+ /**
+ * Initialize the default command for this subsystem.
+ *
+ * This is meant to be the place to call SetDefaultCommand in a subsystem and
+ * will be called on all the subsystems by the CommandBase method before the
+ * program starts running by using the list of all registered Subsystems
+ * inside the Scheduler.
+ *
+ * This should be overridden by a Subsystem that has a default Command
+ */
+ virtual void InitDefaultCommand();
+
+ /**
+ * Gets the name of this Subsystem.
+ *
+ * @return Name
+ */
+ std::string GetName() const;
+
+ /**
+ * Sets the name of this Subsystem.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the subsystem name of this Subsystem.
+ *
+ * @return Subsystem name
+ */
+ std::string GetSubsystem() const;
+
+ /**
+ * Sets the subsystem name of this Subsystem.
+ *
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(const wpi::Twine& subsystem);
+
+ /**
+ * Associate a Sendable with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ void AddChild(const wpi::Twine& name, std::shared_ptr<Sendable> child);
+
+ /**
+ * Associate a Sendable with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ void AddChild(const wpi::Twine& name, Sendable* child);
+
+ /**
+ * Associate a Sendable with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ void AddChild(const wpi::Twine& name, Sendable& child);
+
+ /**
+ * Associate a {@link Sendable} with this Subsystem.
+ *
+ * @param child sendable
+ */
+ void AddChild(std::shared_ptr<Sendable> child);
+
+ /**
+ * Associate a {@link Sendable} with this Subsystem.
+ *
+ * @param child sendable
+ */
+ void AddChild(Sendable* child);
+
+ /**
+ * Associate a {@link Sendable} with this Subsystem.
+ *
+ * @param child sendable
+ */
+ void AddChild(Sendable& child);
+
+ private:
+ /**
+ * Call this to alert Subsystem that the current command is actually the
+ * command.
+ *
+ * Sometimes, the Subsystem is told that it has no command while the Scheduler
+ * is going through the loop, only to be soon after given a new one. This will
+ * avoid that situation.
+ */
+ void ConfirmCommand();
+
+ Command* m_currentCommand = nullptr;
+ bool m_currentCommandChanged = true;
+ Command* m_defaultCommand = nullptr;
+ bool m_initializedDefaultCommand = false;
+
+ public:
+ void InitSendable(SendableBuilder& builder) override;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
new file mode 100644
index 0000000..16f3348
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+/**
+ * A TimedCommand will wait for a timeout before finishing.
+ *
+ * TimedCommand is used to execute a command for a given amount of time.
+ */
+class TimedCommand : public Command {
+ public:
+ /**
+ * Creates a new TimedCommand with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ */
+ TimedCommand(const wpi::Twine& name, double timeout);
+
+ /**
+ * Creates a new WaitCommand with the given timeout.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ */
+ explicit TimedCommand(double timeout);
+
+ /**
+ * Creates a new TimedCommand 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 the command requires
+ */
+ TimedCommand(const wpi::Twine& name, double timeout, Subsystem& subsystem);
+
+ /**
+ * Creates a new WaitCommand with the given timeout.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ * @param subsystem the subsystem that the command requires
+ */
+ TimedCommand(double timeout, Subsystem& subsystem);
+
+ virtual ~TimedCommand() = default;
+
+ TimedCommand(TimedCommand&&) = default;
+ TimedCommand& operator=(TimedCommand&&) = default;
+
+ protected:
+ /**
+ * Ends command when timed out.
+ */
+ bool IsFinished() override;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
new file mode 100644
index 0000000..794b0f7
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/TimedCommand.h"
+
+namespace frc {
+
+class WaitCommand : public TimedCommand {
+ public:
+ /**
+ * Creates a new WaitCommand with the given name and timeout.
+ *
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ */
+ explicit WaitCommand(double timeout);
+
+ /**
+ * Creates a new WaitCommand with the given timeout.
+ *
+ * @param timeout the time (in seconds) before this command "times out"
+ */
+ WaitCommand(const wpi::Twine& name, double timeout);
+
+ virtual ~WaitCommand() = default;
+
+ WaitCommand(WaitCommand&&) = default;
+ WaitCommand& operator=(WaitCommand&&) = default;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h b/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
new file mode 100644
index 0000000..cd1f85a
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+class WaitForChildren : public Command {
+ public:
+ explicit WaitForChildren(double timeout);
+ WaitForChildren(const wpi::Twine& name, double timeout);
+ virtual ~WaitForChildren() = default;
+
+ WaitForChildren(WaitForChildren&&) = default;
+ WaitForChildren& operator=(WaitForChildren&&) = default;
+
+ protected:
+ virtual bool IsFinished();
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h b/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
new file mode 100644
index 0000000..b2f1ffe
--- /dev/null
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc/commands/Command.h"
+
+namespace frc {
+
+class WaitUntilCommand : public Command {
+ public:
+ /**
+ * A WaitCommand will wait until a certain match time before finishing.
+ *
+ * This will wait until the game clock reaches some value, then continue to
+ * the next command.
+ *
+ * @see CommandGroup
+ */
+ explicit WaitUntilCommand(double time);
+
+ WaitUntilCommand(const wpi::Twine& name, double time);
+
+ virtual ~WaitUntilCommand() = default;
+
+ WaitUntilCommand(WaitUntilCommand&&) = default;
+ WaitUntilCommand& operator=(WaitUntilCommand&&) = default;
+
+ protected:
+ /**
+ * Check if we've reached the actual finish time.
+ */
+ virtual bool IsFinished();
+
+ private:
+ double m_time;
+};
+
+} // namespace frc
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
new file mode 100644
index 0000000..d97b352
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import org.junit.jupiter.api.extension.BeforeAllCallback;
+import org.junit.jupiter.api.extension.ExtensionContext;
+import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.sim.DriverStationSim;
+
+public final class MockHardwareExtension implements BeforeAllCallback {
+ private static ExtensionContext getRoot(ExtensionContext context) {
+ return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
+ }
+
+ @Override
+ public void beforeAll(ExtensionContext context) {
+ getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+ initializeHardware();
+ return true;
+ }, Boolean.class);
+ }
+
+ private void initializeHardware() {
+ HAL.initialize(500, 0);
+ DriverStationSim dsSim = new DriverStationSim();
+ dsSim.setDsAttached(true);
+ dsSim.setAutonomous(false);
+ dsSim.setEnabled(true);
+ dsSim.setTest(true);
+
+
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
new file mode 100644
index 0000000..409a74f
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.BeforeEach;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+/**
+ * The basic test for all {@link Command} tests.
+ */
+public abstract class AbstractCommandTest {
+ @BeforeEach
+ void commandSetup() {
+ Scheduler.getInstance().removeAll();
+ Scheduler.getInstance().enable();
+ }
+
+ public class ASubsystem extends Subsystem {
+ Command m_command;
+
+ @Override
+ protected void initDefaultCommand() {
+ if (m_command != null) {
+ setDefaultCommand(m_command);
+ }
+ }
+
+ public void init(Command command) {
+ m_command = command;
+ }
+ }
+
+
+ protected void assertCommandState(MockCommand command, int initialize, int execute,
+ int isFinished, int end, int interrupted) {
+ assertAll(
+ () -> assertEquals(initialize, command.getInitializeCount()),
+ () -> assertEquals(execute, command.getExecuteCount()),
+ () -> assertEquals(isFinished, command.getIsFinishedCount()),
+ () -> assertEquals(end, command.getEndCount()),
+ () -> assertEquals(interrupted, command.getInterruptedCount())
+ );
+ }
+
+ protected void sleep(int time) {
+ assertDoesNotThrow(() -> Thread.sleep(time));
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
new file mode 100644
index 0000000..690eade
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.buttons.InternalButton;
+
+
+/**
+ * Test that covers the {@link edu.wpi.first.wpilibj.buttons.Button} with the {@link Command}
+ * library.
+ */
+class ButtonTest extends AbstractCommandTest {
+ private InternalButton m_button1;
+ private InternalButton m_button2;
+
+ @BeforeEach
+ void setUp() {
+ m_button1 = new InternalButton();
+ m_button2 = new InternalButton();
+ }
+
+ /**
+ * Simple Button Test.
+ */
+ @Test
+ void buttonTest() {
+ final MockCommand command1 = new MockCommand();
+ final MockCommand command2 = new MockCommand();
+ final MockCommand command3 = new MockCommand();
+ final MockCommand command4 = new MockCommand();
+
+ m_button1.whenPressed(command1);
+ m_button1.whenReleased(command2);
+ m_button1.whileHeld(command3);
+ m_button2.whileHeld(command4);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ m_button1.setPressed(true);
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 1, 1, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 2, 2, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ m_button2.setPressed(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 3, 3, 0, 0);
+ assertCommandState(command4, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 0);
+ assertCommandState(command4, 1, 1, 1, 0, 0);
+ m_button1.setPressed(false);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 5, 5, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 6, 6, 0, 0);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 0);
+ m_button2.setPressed(false);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 0);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ command1.cancel();
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 1);
+ assertCommandState(command2, 1, 3, 3, 0, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ command2.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 1);
+ assertCommandState(command2, 1, 4, 4, 1, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 7, 7, 0, 1);
+ assertCommandState(command2, 1, 4, 4, 1, 0);
+ assertCommandState(command3, 1, 4, 4, 0, 1);
+ assertCommandState(command4, 1, 3, 3, 0, 1);
+ }
+
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
new file mode 100644
index 0000000..5bc1949
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandParallelGroupTest extends AbstractCommandTest {
+ /**
+ * Simple Parallel Command Group With 2 commands one command terminates first.
+ */
+ @Test
+ void parallelCommandGroupWithTwoCommandsTest() {
+ final MockCommand command1 = new MockCommand();
+ final MockCommand command2 = new MockCommand();
+
+ final CommandGroup commandGroup = new CommandGroup();
+ commandGroup.addParallel(command1);
+ commandGroup.addParallel(command2);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ commandGroup.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ command1.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 1, 0);
+ assertCommandState(command2, 1, 3, 3, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 1, 0);
+ assertCommandState(command2, 1, 4, 4, 0, 0);
+ command2.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 1, 0);
+ assertCommandState(command2, 1, 5, 5, 1, 0);
+ }
+
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
new file mode 100644
index 0000000..5711032
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandScheduleTest extends AbstractCommandTest {
+ /**
+ * Simple scheduling of a command and making sure the command is run and successfully terminates.
+ */
+ @Test
+ void runAndTerminateTest() {
+ final MockCommand command = new MockCommand();
+ command.start();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ command.setHasFinished(true);
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 1, 0);
+ }
+
+ /**
+ * Simple scheduling of a command and making sure the command is run and cancels correctly.
+ */
+ @Test
+ void runAndCancelTest() {
+ final MockCommand command = new MockCommand();
+ command.start();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 0);
+ command.cancel();
+ assertCommandState(command, 1, 3, 3, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 1);
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
new file mode 100644
index 0000000..680a55d
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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.logging.Logger;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandSequentialGroupTest extends AbstractCommandTest {
+ private static final Logger logger = Logger.getLogger(CommandSequentialGroupTest.class.getName());
+
+ /**
+ * Simple Command Group With 3 commands that all depend on a subsystem. Some commands have a
+ * timeout.
+ */
+ @Test
+ public void testThreeCommandOnSubSystem() {
+ logger.fine("Begining Test");
+ final ASubsystem subsystem = new ASubsystem();
+
+ logger.finest("Creating Mock Command1");
+ final MockCommand command1 = new MockCommand(subsystem);
+ logger.finest("Creating Mock Command2");
+ final MockCommand command2 = new MockCommand(subsystem);
+ logger.finest("Creating Mock Command3");
+ final MockCommand command3 = new MockCommand(subsystem);
+
+ logger.finest("Creating Command Group");
+ final CommandGroup commandGroup = new CommandGroup();
+ commandGroup.addSequential(command1, 1.0);
+ commandGroup.addSequential(command2, 2.0);
+ commandGroup.addSequential(command3);
+
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ logger.finest("Starting Command group");
+ commandGroup.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ sleep(1250); // command 1 timeout
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ assertCommandState(command3, 0, 0, 0, 0, 0);
+ sleep(2500); // command 2 timeout
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 1, 1, 0, 0);
+
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 2, 2, 0, 0);
+ command3.setHasFinished(true);
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 1);
+ assertCommandState(command3, 1, 3, 3, 1, 0);
+ }
+
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
new file mode 100644
index 0000000..91ec0de
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.Test;
+
+/**
+ * Ported from the old CrioTest Classes.
+ */
+class CommandSupersedeTest extends AbstractCommandTest {
+ /**
+ * Testing one command superseding another because of dependencies.
+ */
+ @Test
+ void oneCommandSupersedingAnotherBecauseOfDependenciesTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+ final MockCommand command1 = new MockCommand(subsystem);
+
+ final MockCommand command2 = new MockCommand(subsystem);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command1.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command2.start();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 1);
+ assertCommandState(command2, 1, 3, 3, 0, 0);
+ }
+
+ /**
+ * Testing one command failing superseding another because of dependencies because the first
+ * command cannot be interrupted.
+ */
+ @Test
+ @SuppressWarnings("PMD.NonStaticInitializer")
+ void commandFailingSupersedingBecauseFirstCanNotBeInterruptedTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+ final MockCommand command1 = new MockCommand(subsystem) {
+ {
+ setInterruptible(false);
+ }
+ };
+
+ final MockCommand command2 = new MockCommand(subsystem);
+
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command1.start();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 0, 0, 0, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 1, 1, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 2, 2, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ command2.start();
+ assertCommandState(command1, 1, 3, 3, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command1, 1, 4, 4, 0, 0);
+ assertCommandState(command2, 0, 0, 0, 0, 0);
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
new file mode 100644
index 0000000..229742c
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
@@ -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.command;
+
+import org.junit.jupiter.api.Test;
+
+/**
+ * Test a {@link Command} that times out.
+ */
+class CommandTimeoutTest extends AbstractCommandTest {
+ /**
+ * Command 2 second Timeout Test.
+ */
+ @Test
+ void twoSecondTimeoutTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+
+ final MockCommand command = new MockCommand(subsystem, 2) {
+ @Override
+ public boolean isFinished() {
+ return super.isFinished() || isTimedOut();
+ }
+ };
+
+ command.start();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 3, 3, 0, 0);
+ sleep(2500);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 4, 4, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(command, 1, 4, 4, 1, 0);
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
new file mode 100644
index 0000000..76ec10b
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
@@ -0,0 +1,345 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertSame;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class ConditionalCommandTest extends AbstractCommandTest {
+ MockConditionalCommand m_command;
+ MockConditionalCommand m_commandNull;
+ MockCommand m_onTrue;
+ MockCommand m_onFalse;
+ MockSubsystem m_subsys;
+ Boolean m_condition;
+
+ @BeforeEach
+ void initCommands() {
+ m_subsys = new MockSubsystem();
+ m_onTrue = new MockCommand(m_subsys);
+ m_onFalse = new MockCommand(m_subsys);
+ m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+ m_commandNull = new MockConditionalCommand(m_onTrue, null);
+ }
+
+ protected void assertConditionalCommandState(MockConditionalCommand command, int initialize,
+ int execute, int isFinished, int end,
+ int interrupted) {
+ assertEquals(initialize, command.getInitializeCount());
+ assertEquals(execute, command.getExecuteCount());
+ assertEquals(isFinished, command.getIsFinishedCount());
+ assertEquals(end, command.getEndCount());
+ assertEquals(interrupted, command.getInterruptedCount());
+ }
+
+ @Test
+ void onTrueTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+ assertTrue(m_onTrue.getInitializeCount() > 0, "Did not initialize the true command");
+ assertSame(m_onFalse.getInitializeCount(), 0, "Initialized the false command");
+ }
+
+ @Test
+ void onFalseTest() {
+ m_command.setCondition(false);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onFalse
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onFalse
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 1, 1, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 2, 2, 0, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onFalse.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+ assertTrue(m_onFalse.getInitializeCount() > 0, "Did not initialize the false command");
+ assertSame(m_onTrue.getInitializeCount(), 0, "Initialized the true command");
+ }
+
+ @Test
+ void cancelSubCommandTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.cancel();
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+ }
+
+ @Test
+ void cancelRequiresTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onFalse.start();
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 0, 1);
+ assertCommandState(m_onFalse, 1, 1, 1, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 1);
+ }
+
+ @Test
+ void cancelCondCommandTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_command.cancel();
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 1);
+ }
+
+ @Test
+ void onTrueTwiceTest() {
+ m_command.setCondition(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+ m_onTrue.resetCounters();
+ m_command.resetCounters();
+ m_command.setCondition(true);
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 2, 2, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 0, 0);
+ m_onTrue.setHasFinished(true);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 4, 4, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+ }
+
+ @Test
+ void onTrueInstantTest() {
+ m_command.setCondition(true);
+ m_onTrue.setHasFinished(true);
+
+ Scheduler.getInstance().add(m_command);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onTrue
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+ assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+ }
+
+ @Test
+ void onFalseNullTest() {
+ m_commandNull.setCondition(false);
+
+ Scheduler.getInstance().add(m_commandNull);
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init command and select m_onFalse
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run(); // init m_onFalse
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+ assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
new file mode 100644
index 0000000..3ec2148
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.junit.jupiter.api.Test;
+
+/**
+ * Tests the {@link Command} library.
+ */
+public class DefaultCommandTest extends AbstractCommandTest {
+ /**
+ * Testing of default commands where the interrupting command ends itself.
+ */
+ @Test
+ void defaultCommandWhereTheInteruptingCommandEndsItselfTest() {
+ final ASubsystem subsystem = new ASubsystem();
+
+
+ final MockCommand defaultCommand = new MockCommand(subsystem);
+
+ final MockCommand anotherCommand = new MockCommand(subsystem);
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ subsystem.init(defaultCommand);
+
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+
+ anotherCommand.start();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ anotherCommand.setHasFinished(true);
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 4, 4, 0, 1);
+ assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 5, 5, 0, 1);
+ assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+ }
+
+
+ /**
+ * Testing of default commands where the interrupting command is canceled.
+ */
+ @Test
+ void defaultCommandsInterruptingCommandCanceledTest() {
+ final ASubsystem subsystem = new ASubsystem();
+ final MockCommand defaultCommand = new MockCommand(subsystem);
+ final MockCommand anotherCommand = new MockCommand(subsystem);
+
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ subsystem.init(defaultCommand);
+ subsystem.initDefaultCommand();
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+
+ anotherCommand.start();
+ assertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 1, 1, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ anotherCommand.cancel();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 4, 4, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+ Scheduler.getInstance().run();
+ assertCommandState(defaultCommand, 2, 5, 5, 0, 1);
+ assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+ }
+
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
new file mode 100644
index 0000000..82fe3e2
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* 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 class to simulate a simple command. The command keeps track of how many times each method was
+ * called.
+ */
+public class MockCommand extends Command {
+ private int m_initializeCount;
+ private int m_executeCount;
+ private int m_isFinishedCount;
+ private boolean m_hasFinished;
+ private int m_endCount;
+ private int m_interruptedCount;
+
+ public MockCommand(Subsystem subsys) {
+ super();
+ requires(subsys);
+ }
+
+ public MockCommand(Subsystem subsys, double timeout) {
+ this(subsys);
+ setTimeout(timeout);
+ }
+
+ public MockCommand() {
+ super();
+ }
+
+ @Override
+ protected void initialize() {
+ ++m_initializeCount;
+ }
+
+ @Override
+ protected void execute() {
+ ++m_executeCount;
+ }
+
+ @Override
+ protected boolean isFinished() {
+ ++m_isFinishedCount;
+ return isHasFinished();
+ }
+
+ @Override
+ protected void end() {
+ ++m_endCount;
+ }
+
+ @Override
+ protected void interrupted() {
+ ++m_interruptedCount;
+ }
+
+
+ /**
+ * How many times the initialize method has been called.
+ */
+ public int getInitializeCount() {
+ return m_initializeCount;
+ }
+
+ /**
+ * If the initialize method has been called at least once.
+ */
+ public boolean hasInitialized() {
+ return getInitializeCount() > 0;
+ }
+
+ /**
+ * How many time the execute method has been called.
+ */
+ public int getExecuteCount() {
+ return m_executeCount;
+ }
+
+ /**
+ * How many times the isFinished method has been called.
+ */
+ public int getIsFinishedCount() {
+ return m_isFinishedCount;
+ }
+
+ /**
+ * Get what value the isFinished method will return.
+ *
+ * @return what value the isFinished method will return.
+ */
+ public boolean isHasFinished() {
+ return m_hasFinished;
+ }
+
+ /**
+ * Set what value the isFinished method will return.
+ *
+ * @param hasFinished set what value the isFinished method will return.
+ */
+ public void setHasFinished(boolean hasFinished) {
+ m_hasFinished = hasFinished;
+ }
+
+ /**
+ * How many times the end method has been called.
+ */
+ public int getEndCount() {
+ return m_endCount;
+ }
+
+ /**
+ * If the end method has been called at least once.
+ */
+ public boolean hasEnd() {
+ return getEndCount() > 0;
+ }
+
+ /**
+ * How many times the interrupted method has been called.
+ */
+ public int getInterruptedCount() {
+ return m_interruptedCount;
+ }
+
+ /**
+ * If the interrupted method has been called at least once.
+ */
+ public boolean hasInterrupted() {
+ return getInterruptedCount() > 0;
+ }
+
+ /**
+ * Reset internal counters.
+ */
+ public void resetCounters() {
+ m_initializeCount = 0;
+ m_executeCount = 0;
+ m_isFinishedCount = 0;
+ m_hasFinished = false;
+ m_endCount = 0;
+ m_interruptedCount = 0;
+ }
+
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
new file mode 100644
index 0000000..688afa3
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+public class MockConditionalCommand extends ConditionalCommand {
+ private boolean m_condition;
+ private int m_initializeCount;
+ private int m_executeCount;
+ private int m_isFinishedCount;
+ private int m_endCount;
+ private int m_interruptedCount;
+
+ public MockConditionalCommand(MockCommand onTrue, MockCommand onFalse) {
+ super(onTrue, onFalse);
+ }
+
+ @Override
+ protected boolean condition() {
+ return m_condition;
+ }
+
+ public void setCondition(boolean condition) {
+ this.m_condition = condition;
+ }
+
+ @Override
+ protected void initialize() {
+ ++m_initializeCount;
+ }
+
+ @Override
+ protected void execute() {
+ ++m_executeCount;
+ }
+
+ @Override
+ protected boolean isFinished() {
+ ++m_isFinishedCount;
+ return super.isFinished();
+ }
+
+ @Override
+ protected void end() {
+ ++m_endCount;
+ }
+
+ @Override
+ protected void interrupted() {
+ ++m_interruptedCount;
+ }
+
+
+ /**
+ * How many times the initialize method has been called.
+ */
+ public int getInitializeCount() {
+ return m_initializeCount;
+ }
+
+ /**
+ * If the initialize method has been called at least once.
+ */
+ public boolean hasInitialized() {
+ return getInitializeCount() > 0;
+ }
+
+ /**
+ * How many time the execute method has been called.
+ */
+ public int getExecuteCount() {
+ return m_executeCount;
+ }
+
+ /**
+ * How many times the isFinished method has been called.
+ */
+ public int getIsFinishedCount() {
+ return m_isFinishedCount;
+ }
+
+ /**
+ * How many times the end method has been called.
+ */
+ public int getEndCount() {
+ return m_endCount;
+ }
+
+ /**
+ * If the end method has been called at least once.
+ */
+ public boolean hasEnd() {
+ return getEndCount() > 0;
+ }
+
+ /**
+ * How many times the interrupted method has been called.
+ */
+ public int getInterruptedCount() {
+ return m_interruptedCount;
+ }
+
+ /**
+ * If the interrupted method has been called at least once.
+ */
+ public boolean hasInterrupted() {
+ return getInterruptedCount() > 0;
+ }
+
+ /**
+ * Reset internal counters.
+ */
+ public void resetCounters() {
+ m_condition = false;
+ m_initializeCount = 0;
+ m_executeCount = 0;
+ m_isFinishedCount = 0;
+ m_endCount = 0;
+ m_interruptedCount = 0;
+ }
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
new file mode 100644
index 0000000..604df89
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+/**
+ * A class to simulate a simple subsystem.
+ */
+public class MockSubsystem extends Subsystem {
+ @Override
+ protected void initDefaultCommand() {}
+}
diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
new file mode 100644
index 0000000..16f088f
--- /dev/null
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* 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.shuffleboard;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+@SuppressWarnings({"PMD.TooManyMethods"})
+public class ShuffleboardTabTest {
+ private NetworkTableInstance m_ntInstance;
+ private ShuffleboardTab m_tab;
+ private ShuffleboardInstance m_instance;
+
+ @BeforeEach
+ void setup() {
+ m_ntInstance = NetworkTableInstance.create();
+ m_instance = new ShuffleboardInstance(m_ntInstance);
+ m_tab = m_instance.getTab("Tab");
+ }
+
+ @AfterEach
+ void tearDown() {
+ m_ntInstance.close();
+ }
+
+ @Test
+ void testAddDouble() {
+ NetworkTableEntry entry = m_tab.add("Double", 1.0).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Double", entry.getName()),
+ () -> assertEquals(1.0, entry.getValue().getDouble()));
+ }
+
+ @Test
+ void testAddInteger() {
+ NetworkTableEntry entry = m_tab.add("Int", 1).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Int", entry.getName()),
+ () -> assertEquals(1.0, entry.getValue().getDouble()));
+ }
+
+ @Test
+ void testAddLong() {
+ NetworkTableEntry entry = m_tab.add("Long", 1L).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Long", entry.getName()),
+ () -> assertEquals(1.0, entry.getValue().getDouble()));
+ }
+
+
+ @Test
+ void testAddBoolean() {
+ NetworkTableEntry entry = m_tab.add("Bool", false).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/Bool", entry.getName()),
+ () -> assertFalse(entry.getValue().getBoolean()));
+ }
+
+ @Test
+ void testAddString() {
+ NetworkTableEntry entry = m_tab.add("String", "foobar").getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/String", entry.getName()),
+ () -> assertEquals("foobar", entry.getValue().getString()));
+ }
+
+ @Test
+ void testAddNamedSendableWithProperties() {
+ Sendable sendable = new InstantCommand("Command");
+ String widgetType = "Command Widget";
+ m_tab.add(sendable)
+ .withWidget(widgetType)
+ .withProperties(mapOf("foo", 1234, "bar", "baz"));
+
+ m_instance.update();
+ String meta = "/Shuffleboard/.metadata/Tab/Command";
+
+ assertAll(
+ () -> assertEquals(1234,
+ m_ntInstance.getEntry(meta + "/Properties/foo").getDouble(-1),
+ "Property 'foo' not set correctly"),
+ () -> assertEquals("baz",
+ m_ntInstance.getEntry(meta + "/Properties/bar").getString(null),
+ "Property 'bar' not set correctly"),
+ () -> assertEquals(widgetType,
+ m_ntInstance.getEntry(meta + "/PreferredComponent").getString(null),
+ "Preferred component not set correctly"));
+ }
+
+ @Test
+ void testAddNumberArray() {
+ NetworkTableEntry entry = m_tab.add("DoubleArray", new double[]{1, 2, 3}).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/DoubleArray", entry.getName()),
+ () -> assertArrayEquals(new double[]{1, 2, 3}, entry.getValue().getDoubleArray()));
+ }
+
+ @Test
+ void testAddBooleanArray() {
+ NetworkTableEntry entry = m_tab.add("BoolArray", new boolean[]{true, false}).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/BoolArray", entry.getName()),
+ () -> assertArrayEquals(new boolean[]{true, false}, entry.getValue().getBooleanArray()));
+ }
+
+ @Test
+ void testAddStringArray() {
+ NetworkTableEntry entry = m_tab.add("StringArray", new String[]{"foo", "bar"}).getEntry();
+ assertAll(
+ () -> assertEquals("/Shuffleboard/Tab/StringArray", entry.getName()),
+ () -> assertArrayEquals(new String[]{"foo", "bar"}, entry.getValue().getStringArray()));
+ }
+
+ @Test
+ void testTitleDuplicates() {
+ m_tab.add("foo", "bar");
+ assertThrows(IllegalArgumentException.class, () -> m_tab.add("foo", "baz"));
+ }
+
+ /**
+ * Stub for Java 9 {@code Map.of()}.
+ */
+ @SuppressWarnings({"unchecked", "PMD"})
+ private static <K, V> Map<K, V> mapOf(Object... entries) {
+ Map<K, V> map = new HashMap<>();
+ for (int i = 0; i < entries.length; i += 2) {
+ map.put((K) entries[i], (V) entries[i + 1]);
+ }
+ return map;
+ }
+
+}
diff --git a/wpilibOldCommands/src/test/native/cpp/main.cpp b/wpilibOldCommands/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..c6b6c58
--- /dev/null
+++ b/wpilibOldCommands/src/test/native/cpp/main.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/HALBase.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+ HAL_Initialize(500, 0);
+ ::testing::InitGoogleTest(&argc, argv);
+ int ret = RUN_ALL_TESTS();
+ return ret;
+}
diff --git a/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
new file mode 100644
index 0000000..e0e3db0
--- /dev/null
+++ b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/commands/InstantCommand.h"
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class ShuffleboardTabTest : public testing::Test {
+ void SetUp() override {
+ m_ntInstance = nt::NetworkTableInstance::Create();
+ m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
+ m_tab = &(m_instance->GetTab("Tab"));
+ }
+
+ protected:
+ nt::NetworkTableInstance m_ntInstance;
+ ShuffleboardTab* m_tab;
+ std::unique_ptr<detail::ShuffleboardInstance> m_instance;
+};
+
+TEST_F(ShuffleboardTabTest, AddDouble) {
+ auto entry = m_tab->Add("Double", 1.0).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Double", entry.GetName());
+ EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
+}
+
+TEST_F(ShuffleboardTabTest, AddInteger) {
+ auto entry = m_tab->Add("Int", 1).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Int", entry.GetName());
+ EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
+}
+
+TEST_F(ShuffleboardTabTest, AddBoolean) {
+ auto entry = m_tab->Add("Bool", false).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Bool", entry.GetName());
+ EXPECT_FALSE(entry.GetValue()->GetBoolean());
+}
+
+TEST_F(ShuffleboardTabTest, AddString) {
+ auto entry = m_tab->Add("String", "foobar").GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/String", entry.GetName());
+ EXPECT_EQ("foobar", entry.GetValue()->GetString());
+}
+
+TEST_F(ShuffleboardTabTest, AddNamedSendableWithProperties) {
+ InstantCommand sendable("Command");
+ std::string widgetType = "Command Widget";
+ wpi::StringMap<std::shared_ptr<nt::Value>> map;
+ map.try_emplace("foo", nt::Value::MakeDouble(1234));
+ map.try_emplace("bar", nt::Value::MakeString("baz"));
+ m_tab->Add(sendable).WithWidget(widgetType).WithProperties(map);
+
+ m_instance->Update();
+ std::string meta = "/Shuffleboard/.metadata/Tab/Command";
+
+ EXPECT_EQ(1234, m_ntInstance.GetEntry(meta + "/Properties/foo").GetDouble(-1))
+ << "Property 'foo' not set correctly";
+ EXPECT_EQ("baz",
+ m_ntInstance.GetEntry(meta + "/Properties/bar").GetString(""))
+ << "Property 'bar' not set correctly";
+ EXPECT_EQ(widgetType,
+ m_ntInstance.GetEntry(meta + "/PreferredComponent").GetString(""))
+ << "Preferred component not set correctly";
+}
+
+TEST_F(ShuffleboardTabTest, AddNumberArray) {
+ std::array<double, 3> expect = {{1.0, 2.0, 3.0}};
+ auto entry = m_tab->Add("DoubleArray", expect).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/DoubleArray", entry.GetName());
+
+ auto actual = entry.GetValue()->GetDoubleArray();
+ EXPECT_EQ(expect.size(), actual.size());
+ for (size_t i = 0; i < expect.size(); i++) {
+ EXPECT_FLOAT_EQ(expect[i], actual[i]);
+ }
+}
+
+TEST_F(ShuffleboardTabTest, AddBooleanArray) {
+ std::array<bool, 2> expect = {{true, false}};
+ auto entry = m_tab->Add("BoolArray", expect).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/BoolArray", entry.GetName());
+
+ auto actual = entry.GetValue()->GetBooleanArray();
+ EXPECT_EQ(expect.size(), actual.size());
+ for (size_t i = 0; i < expect.size(); i++) {
+ EXPECT_EQ(expect[i], actual[i]);
+ }
+}
+
+TEST_F(ShuffleboardTabTest, AddStringArray) {
+ std::array<std::string, 2> expect = {{"foo", "bar"}};
+ auto entry = m_tab->Add("StringArray", expect).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/StringArray", entry.GetName());
+
+ auto actual = entry.GetValue()->GetStringArray();
+ EXPECT_EQ(expect.size(), actual.size());
+ for (size_t i = 0; i < expect.size(); i++) {
+ EXPECT_EQ(expect[i], actual[i]);
+ }
+}
diff --git a/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
new file mode 100644
index 0000000..0b06d7f
--- /dev/null
+++ b/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/commands/InstantCommand.h"
+#include "frc/shuffleboard/BuiltInWidgets.h"
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+#include "frc/shuffleboard/ShuffleboardWidget.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class ShuffleboardWidgetTest : public testing::Test {
+ void SetUp() override {
+ m_ntInstance = nt::NetworkTableInstance::Create();
+ m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
+ m_tab = &(m_instance->GetTab("Tab"));
+ }
+
+ protected:
+ nt::NetworkTableInstance m_ntInstance;
+ ShuffleboardTab* m_tab;
+ std::unique_ptr<detail::ShuffleboardInstance> m_instance;
+};
+
+TEST_F(ShuffleboardWidgetTest, UseBuiltInWidget) {
+ auto entry =
+ m_tab->Add("Name", "").WithWidget(BuiltInWidgets::kTextView).GetEntry();
+ EXPECT_EQ("/Shuffleboard/Tab/Name", entry.GetName())
+ << "The widget entry has the wrong name";
+}
+
+TEST_F(ShuffleboardWidgetTest, WithProperties) {
+ wpi::StringMap<std::shared_ptr<nt::Value>> properties{
+ std::make_pair("min", nt::Value::MakeDouble(0)),
+ std::make_pair("max", nt::Value::MakeDouble(1))};
+ auto entry =
+ m_tab->Add("WithProperties", "").WithProperties(properties).GetEntry();
+
+ // Update the instance to generate
+ // the metadata entries for the widget properties
+ m_instance->Update();
+
+ auto propertiesTable = m_ntInstance.GetTable(
+ "/Shuffleboard/.metadata/Tab/WithProperties/Properties");
+
+ EXPECT_EQ("/Shuffleboard/Tab/WithProperties", entry.GetName())
+ << "The widget entry has the wrong name";
+ EXPECT_FLOAT_EQ(0, propertiesTable->GetEntry("min").GetDouble(-1))
+ << "The 'min' property should be 0";
+ EXPECT_FLOAT_EQ(1, propertiesTable->GetEntry("max").GetDouble(-1))
+ << "The 'max' property should be 1";
+}
diff --git a/wpilibOldCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibOldCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..981f170
--- /dev/null
+++ b/wpilibOldCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj.MockHardwareExtension