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/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java b/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
new file mode 100644
index 0000000..ca6ce63
--- /dev/null
+++ b/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/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.wpilibj2.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/wpilibNewCommands/src/dev/native/cpp/main.cpp b/wpilibNewCommands/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..5312a1d
--- /dev/null
+++ b/wpilibNewCommands/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/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
new file mode 100644
index 0000000..6cca974
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -0,0 +1,311 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.BooleanSupplier;
+
+/**
+ * A state machine representing a complete action to be performed by the robot. Commands are
+ * run by the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to
+ * build complicated multi-step actions without the need to roll the state machine logic themselves.
+ *
+ * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
+ * specified explicitly from the command implementation.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public interface Command {
+
+ /**
+ * The initial subroutine of a command. Called once when the command is initially scheduled.
+ */
+ default void initialize() {
+ }
+
+ /**
+ * The main body of a command. Called repeatedly while the command is scheduled.
+ */
+ default void execute() {
+ }
+
+ /**
+ * The action to take when the command ends. Called when either the command finishes normally,
+ * or when it interrupted/canceled.
+ *
+ * @param interrupted whether the command was interrupted/canceled
+ */
+ default void end(boolean interrupted) {
+ }
+
+ /**
+ * Whether the command has finished. Once a command finishes, the scheduler will call its
+ * end() method and un-schedule it.
+ *
+ * @return whether the command has finished.
+ */
+ default boolean isFinished() {
+ return false;
+ }
+
+ /**
+ * Specifies the set of subsystems used by this command. Two commands cannot use the same
+ * subsystem at the same time. If the command is scheduled as interruptible and another
+ * command is scheduled that shares a requirement, the command will be interrupted. Else,
+ * the command will not be scheduled. If no subsystems are required, return an empty set.
+ *
+ * <p>Note: it is recommended that user implementations contain the requirements as a field,
+ * and return that field here, rather than allocating a new set every time this is called.
+ *
+ * @return the set of subsystems that are required
+ */
+ Set<Subsystem> getRequirements();
+
+ /**
+ * Decorates this command with a timeout. If the specified timeout is exceeded before the command
+ * finishes normally, the command will be interrupted and un-scheduled. Note that the
+ * timeout only applies to the command returned by this method; the calling command is
+ * not itself changed.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param seconds the timeout duration
+ * @return the command with the timeout added
+ */
+ default ParallelRaceGroup withTimeout(double seconds) {
+ return new ParallelRaceGroup(this, new WaitCommand(seconds));
+ }
+
+ /**
+ * Decorates this command with an interrupt condition. If the specified condition becomes true
+ * before the command finishes normally, the command will be interrupted and un-scheduled.
+ * Note that this only applies to the command returned by this method; the calling command
+ * is not itself changed.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param condition the interrupt condition
+ * @return the command with the interrupt condition added
+ */
+ default ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
+ return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
+ }
+
+ /**
+ * Decorates this command with a runnable to run before this command starts.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the required subsystems
+ * @return the decorated command
+ */
+ default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
+ return new SequentialCommandGroup(new InstantCommand(toRun, requirements), this);
+ }
+
+ /**
+ * Decorates this command with a runnable to run after the command finishes.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the required subsystems
+ * @return the decorated command
+ */
+ default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
+ return new SequentialCommandGroup(this, new InstantCommand(toRun, requirements));
+ }
+
+ /**
+ * Decorates this command with a set of commands to run after it in sequence. Often more
+ * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param next the commands to run next
+ * @return the decorated command
+ */
+ default SequentialCommandGroup andThen(Command... next) {
+ SequentialCommandGroup group = new SequentialCommandGroup(this);
+ group.addCommands(next);
+ return group;
+ }
+
+ /**
+ * Decorates this command with a set of commands to run parallel to it, ending when the calling
+ * command ends and interrupting all the others. Often more convenient/less-verbose than
+ * constructing a new {@link ParallelDeadlineGroup} explicitly.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param parallel the commands to run in parallel
+ * @return the decorated command
+ */
+ default ParallelDeadlineGroup deadlineWith(Command... parallel) {
+ return new ParallelDeadlineGroup(this, parallel);
+ }
+
+ /**
+ * Decorates this command with a set of commands to run parallel to it, ending when the last
+ * command ends. Often more convenient/less-verbose than constructing a new
+ * {@link ParallelCommandGroup} explicitly.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param parallel the commands to run in parallel
+ * @return the decorated command
+ */
+ default ParallelCommandGroup alongWith(Command... parallel) {
+ ParallelCommandGroup group = new ParallelCommandGroup(this);
+ group.addCommands(parallel);
+ return group;
+ }
+
+ /**
+ * Decorates this command with a set of commands to run parallel to it, ending when the first
+ * command ends. Often more convenient/less-verbose than constructing a new
+ * {@link ParallelRaceGroup} explicitly.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @param parallel the commands to run in parallel
+ * @return the decorated command
+ */
+ default ParallelRaceGroup raceWith(Command... parallel) {
+ ParallelRaceGroup group = new ParallelRaceGroup(this);
+ group.addCommands(parallel);
+ return group;
+ }
+
+ /**
+ * Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated
+ * command can still be interrupted or canceled.
+ *
+ * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+ * cannot be used independently after being decorated, or be re-decorated with a different
+ * decorator, unless it is manually cleared from the list of grouped commands with
+ * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be
+ * further decorated without issue.
+ *
+ * @return the decorated command
+ */
+ default PerpetualCommand perpetually() {
+ return new PerpetualCommand(this);
+ }
+
+ /**
+ * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}.
+ * This is useful for "forking off" from command groups when the user does not wish to extend
+ * the command's requirements to the entire command group.
+ *
+ * @return the decorated command
+ */
+ default ProxyScheduleCommand asProxy() {
+ return new ProxyScheduleCommand(this);
+ }
+
+ /**
+ * Schedules this command.
+ *
+ * @param interruptible whether this command can be interrupted by another command that
+ * shares one of its requirements
+ */
+ default void schedule(boolean interruptible) {
+ CommandScheduler.getInstance().schedule(interruptible, this);
+ }
+
+ /**
+ * Schedules this command, defaulting to interruptible.
+ */
+ default void schedule() {
+ schedule(true);
+ }
+
+ /**
+ * Cancels this command. Will call the command's interrupted() method.
+ * Commands will be canceled even if they are not marked as interruptible.
+ */
+ default void cancel() {
+ CommandScheduler.getInstance().cancel(this);
+ }
+
+ /**
+ * Whether or not the command is currently scheduled. Note that this does not detect whether
+ * the command is being run by a CommandGroup, only whether it is directly being run by
+ * the scheduler.
+ *
+ * @return Whether the command is scheduled.
+ */
+ default boolean isScheduled() {
+ return CommandScheduler.getInstance().isScheduled(this);
+ }
+
+ /**
+ * Whether the command requires a given subsystem. Named "hasRequirement" rather than "requires"
+ * to avoid confusion with
+ * {@link edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
+ * - this may be able to be changed in a few years.
+ *
+ * @param requirement the subsystem to inquire about
+ * @return whether the subsystem is required
+ */
+ default boolean hasRequirement(Subsystem requirement) {
+ return getRequirements().contains(requirement);
+ }
+
+ /**
+ * Whether the given command should run when the robot is disabled. Override to return true
+ * if the command should run when disabled.
+ *
+ * @return whether the command should run when the robot is disabled
+ */
+ default boolean runsWhenDisabled() {
+ return false;
+ }
+
+ /**
+ * Gets the name of this Command.
+ *
+ * @return Name
+ */
+ default String getName() {
+ return this.getClass().getSimpleName();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
new file mode 100644
index 0000000..ba2c68f
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.HashSet;
+import java.util.Set;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A {@link Sendable} base class for {@link Command}s.
+ */
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class CommandBase implements Sendable, Command {
+
+ protected Set<Subsystem> m_requirements = new HashSet<>();
+
+ protected CommandBase() {
+ String name = getClass().getName();
+ SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
+ }
+
+ /**
+ * Adds the specified requirements to the command.
+ *
+ * @param requirements the requirements to add
+ */
+ public final void addRequirements(Subsystem... requirements) {
+ m_requirements.addAll(Set.of(requirements));
+ }
+
+ @Override
+ public Set<Subsystem> getRequirements() {
+ return m_requirements;
+ }
+
+ @Override
+ public String getName() {
+ return SendableRegistry.getName(this);
+ }
+
+ /**
+ * Sets the name of this Command.
+ *
+ * @param name name
+ */
+ @Override
+ public void setName(String name) {
+ SendableRegistry.setName(this, name);
+ }
+
+ /**
+ * Gets the subsystem name of this Command.
+ *
+ * @return Subsystem name
+ */
+ @Override
+ public String getSubsystem() {
+ return SendableRegistry.getSubsystem(this);
+ }
+
+ /**
+ * Sets the subsystem name of this Command.
+ *
+ * @param subsystem subsystem name
+ */
+ @Override
+ public void setSubsystem(String subsystem) {
+ SendableRegistry.setSubsystem(this, subsystem);
+ }
+
+ /**
+ * Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
+ *
+ * @param builder the builder used to construct this sendable
+ */
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Command");
+ builder.addStringProperty(".name", this::getName, null);
+ builder.addBooleanProperty("running", this::isScheduled, value -> {
+ if (value) {
+ if (!isScheduled()) {
+ schedule();
+ }
+ } else {
+ if (isScheduled()) {
+ cancel();
+ }
+ }
+ });
+ builder.addBooleanProperty(".isParented",
+ () -> CommandGroupBase.getGroupedCommands().contains(this), null);
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
new file mode 100644
index 0000000..6ac0667
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Set;
+import java.util.WeakHashMap;
+
+/**
+ * A base for CommandGroups. Statically tracks commands that have been allocated to groups to
+ * ensure those commands are not also used independently, which can result in inconsistent command
+ * state and unpredictable execution.
+ */
+public abstract class CommandGroupBase extends CommandBase implements Command {
+ private static final Set<Command> m_groupedCommands =
+ Collections.newSetFromMap(new WeakHashMap<>());
+
+ static void registerGroupedCommands(Command... commands) {
+ m_groupedCommands.addAll(Set.of(commands));
+ }
+
+ /**
+ * Clears the list of grouped commands, allowing all commands to be freely used again.
+ *
+ * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not
+ * use this unless you fully understand what you are doing.
+ */
+ public static void clearGroupedCommands() {
+ m_groupedCommands.clear();
+ }
+
+ /**
+ * Removes a single command from the list of grouped commands, allowing it to be freely used
+ * again.
+ *
+ * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not
+ * use this unless you fully understand what you are doing.
+ *
+ * @param command the command to remove from the list of grouped commands
+ */
+ public static void clearGroupedCommand(Command command) {
+ m_groupedCommands.remove(command);
+ }
+
+ /**
+ * Requires that the specified commands not have been already allocated to a CommandGroup. Throws
+ * an {@link IllegalArgumentException} if commands have been allocated.
+ *
+ * @param commands The commands to check
+ */
+ public static void requireUngrouped(Command... commands) {
+ requireUngrouped(Set.of(commands));
+ }
+
+ /**
+ * Requires that the specified commands not have been already allocated to a CommandGroup. Throws
+ * an {@link IllegalArgumentException} if commands have been allocated.
+ *
+ * @param commands The commands to check
+ */
+ public static void requireUngrouped(Collection<Command> commands) {
+ if (!Collections.disjoint(commands, getGroupedCommands())) {
+ throw new IllegalArgumentException("Commands cannot be added to more than one CommandGroup");
+ }
+ }
+
+ static Set<Command> getGroupedCommands() {
+ return m_groupedCommands;
+ }
+
+ /**
+ * Adds the given commands to the command group.
+ *
+ * @param commands The commands to add.
+ */
+ public abstract void addCommands(Command... commands);
+
+ /**
+ * Factory method for {@link SequentialCommandGroup}, included for brevity/convenience.
+ *
+ * @param commands the commands to include
+ * @return the command group
+ */
+ public static CommandGroupBase sequence(Command... commands) {
+ return new SequentialCommandGroup(commands);
+ }
+
+ /**
+ * Factory method for {@link ParallelCommandGroup}, included for brevity/convenience.
+ *
+ * @param commands the commands to include
+ * @return the command group
+ */
+ public static CommandGroupBase parallel(Command... commands) {
+ return new ParallelCommandGroup(commands);
+ }
+
+ /**
+ * Factory method for {@link ParallelRaceGroup}, included for brevity/convenience.
+ *
+ * @param commands the commands to include
+ * @return the command group
+ */
+ public static CommandGroupBase race(Command... commands) {
+ return new ParallelRaceGroup(commands);
+ }
+
+ /**
+ * Factory method for {@link ParallelDeadlineGroup}, included for brevity/convenience.
+ *
+ * @param deadline the deadline command
+ * @param commands the commands to include
+ * @return the command group
+ */
+ public static CommandGroupBase deadline(Command deadline, Command... commands) {
+ return new ParallelDeadlineGroup(deadline, commands);
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
new file mode 100644
index 0000000..b34f20e
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -0,0 +1,509 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Iterator;
+import java.util.LinkedHashMap;
+import java.util.LinkedHashSet;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+import java.util.function.Consumer;
+
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link
+ * CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands
+ * synchronously from the main loop. Subsystems should be registered with the scheduler using
+ * {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link
+ * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
+ */
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
+public final class CommandScheduler implements Sendable {
+ /**
+ * The Singleton Instance.
+ */
+ private static CommandScheduler instance;
+
+ /**
+ * Returns the Scheduler instance.
+ *
+ * @return the instance
+ */
+ public static synchronized CommandScheduler getInstance() {
+ if (instance == null) {
+ instance = new CommandScheduler();
+ }
+ return instance;
+ }
+
+ //A map from commands to their scheduling state. Also used as a set of the currently-running
+ //commands.
+ private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
+
+ //A map from required subsystems to their requiring commands. Also used as a set of the
+ //currently-required subsystems.
+ private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
+
+ //A map from subsystems registered with the scheduler to their default commands. Also used
+ //as a list of currently-registered subsystems.
+ private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>();
+
+ //The set of currently-registered buttons that will be polled every iteration.
+ private final Collection<Runnable> m_buttons = new LinkedHashSet<>();
+
+ private boolean m_disabled;
+
+ //NetworkTable entries for use in Sendable impl
+ private NetworkTableEntry m_namesEntry;
+ private NetworkTableEntry m_idsEntry;
+ private NetworkTableEntry m_cancelEntry;
+
+ //Lists of user-supplied actions to be executed on scheduling events for every command.
+ private final List<Consumer<Command>> m_initActions = new ArrayList<>();
+ private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
+ private final List<Consumer<Command>> m_interruptActions = new ArrayList<>();
+ private final List<Consumer<Command>> m_finishActions = new ArrayList<>();
+
+ // Flag and queues for avoiding ConcurrentModificationException if commands are
+ // scheduled/canceled during run
+ private boolean m_inRunLoop;
+ private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
+ private final List<Command> m_toCancel = new ArrayList<>();
+
+
+ CommandScheduler() {
+ HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+ SendableRegistry.addLW(this, "Scheduler");
+ }
+
+ /**
+ * Adds a button binding to the scheduler, which will be polled to schedule commands.
+ *
+ * @param button The button to add
+ */
+ public void addButton(Runnable button) {
+ m_buttons.add(button);
+ }
+
+ /**
+ * Removes all button bindings from the scheduler.
+ */
+ public void clearButtons() {
+ m_buttons.clear();
+ }
+
+ /**
+ * Initializes a given command, adds its requirements to the list, and performs the init actions.
+ *
+ * @param command The command to initialize
+ * @param interruptible Whether the command is interruptible
+ * @param requirements The command requirements
+ */
+ private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
+ command.initialize();
+ CommandState scheduledCommand = new CommandState(interruptible);
+ m_scheduledCommands.put(command, scheduledCommand);
+ for (Consumer<Command> action : m_initActions) {
+ action.accept(command);
+ }
+ for (Subsystem requirement : requirements) {
+ m_requirements.put(requirement, command);
+ }
+ }
+
+ /**
+ * Schedules a command for execution. Does nothing if the command is already scheduled. If a
+ * command's requirements are not available, it will only be started if all the commands currently
+ * using those requirements have been scheduled as interruptible. If this is the case, they will
+ * be interrupted and the command will be scheduled.
+ *
+ * @param interruptible whether this command can be interrupted
+ * @param command the command to schedule
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ private void schedule(boolean interruptible, Command command) {
+ if (m_inRunLoop) {
+ m_toSchedule.put(command, interruptible);
+ return;
+ }
+
+ if (CommandGroupBase.getGroupedCommands().contains(command)) {
+ throw new IllegalArgumentException(
+ "A command that is part of a command group cannot be independently scheduled");
+ }
+
+ //Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
+ //run when disabled, or the command is already scheduled.
+ if (m_disabled || (RobotState.isDisabled() && !command.runsWhenDisabled())
+ || m_scheduledCommands.containsKey(command)) {
+ return;
+ }
+
+ Set<Subsystem> requirements = command.getRequirements();
+
+ //Schedule the command if the requirements are not currently in-use.
+ if (Collections.disjoint(m_requirements.keySet(), requirements)) {
+ initCommand(command, interruptible, requirements);
+ } else {
+ //Else check if the requirements that are in use have all have interruptible commands,
+ //and if so, interrupt those commands and schedule the new command.
+ for (Subsystem requirement : requirements) {
+ if (m_requirements.containsKey(requirement)
+ && !m_scheduledCommands.get(m_requirements.get(requirement)).isInterruptible()) {
+ return;
+ }
+ }
+ for (Subsystem requirement : requirements) {
+ if (m_requirements.containsKey(requirement)) {
+ cancel(m_requirements.get(requirement));
+ }
+ }
+ initCommand(command, interruptible, requirements);
+ }
+ }
+
+ /**
+ * Schedules multiple commands for execution. Does nothing if the command is already scheduled.
+ * If a command's requirements are not available, it will only be started if all the commands
+ * currently using those requirements have been scheduled as interruptible. If this is the case,
+ * they will be interrupted and the command will be scheduled.
+ *
+ * @param interruptible whether the commands should be interruptible
+ * @param commands the commands to schedule
+ */
+ public void schedule(boolean interruptible, Command... commands) {
+ for (Command command : commands) {
+ schedule(interruptible, command);
+ }
+ }
+
+ /**
+ * Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
+ * if the command is already scheduled.
+ *
+ * @param commands the commands to schedule
+ */
+ public void schedule(Command... commands) {
+ schedule(true, commands);
+ }
+
+ /**
+ * Runs a single iteration of the scheduler. The execution occurs in the following order:
+ *
+ * <p>Subsystem periodic methods are called.
+ *
+ * <p>Button bindings are polled, and new commands are scheduled from them.
+ *
+ * <p>Currently-scheduled commands are executed.
+ *
+ * <p>End conditions are checked on currently-scheduled commands, and commands that are finished
+ * have their end methods called and are removed.
+ *
+ * <p>Any subsystems not being used as requirements have their default methods started.
+ */
+ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+ public void run() {
+ if (m_disabled) {
+ return;
+ }
+
+ //Run the periodic method of all registered subsystems.
+ for (Subsystem subsystem : m_subsystems.keySet()) {
+ subsystem.periodic();
+ }
+
+ //Poll buttons for new commands to add.
+ for (Runnable button : m_buttons) {
+ button.run();
+ }
+
+ m_inRunLoop = true;
+ //Run scheduled commands, remove finished commands.
+ for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
+ iterator.hasNext(); ) {
+ Command command = iterator.next();
+
+ if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
+ command.end(true);
+ for (Consumer<Command> action : m_interruptActions) {
+ action.accept(command);
+ }
+ m_requirements.keySet().removeAll(command.getRequirements());
+ iterator.remove();
+ continue;
+ }
+
+ command.execute();
+ for (Consumer<Command> action : m_executeActions) {
+ action.accept(command);
+ }
+ if (command.isFinished()) {
+ command.end(false);
+ for (Consumer<Command> action : m_finishActions) {
+ action.accept(command);
+ }
+ iterator.remove();
+
+ m_requirements.keySet().removeAll(command.getRequirements());
+ }
+ }
+ m_inRunLoop = false;
+
+ //Schedule/cancel commands from queues populated during loop
+ for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
+ schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
+ }
+
+ for (Command command : m_toCancel) {
+ cancel(command);
+ }
+
+ m_toSchedule.clear();
+ m_toCancel.clear();
+
+ //Add default commands for un-required registered subsystems.
+ for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
+ if (!m_requirements.containsKey(subsystemCommand.getKey())
+ && subsystemCommand.getValue() != null) {
+ schedule(subsystemCommand.getValue());
+ }
+ }
+ }
+
+ /**
+ * Registers subsystems with the scheduler. This must be called for the subsystem's periodic
+ * block to run when the scheduler is run, and for the subsystem's default command to be
+ * scheduled. It is recommended to call this from the constructor of your subsystem
+ * implementations.
+ *
+ * @param subsystems the subsystem to register
+ */
+ public void registerSubsystem(Subsystem... subsystems) {
+ for (Subsystem subsystem : subsystems) {
+ m_subsystems.put(subsystem, null);
+ }
+ }
+
+ /**
+ * Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic
+ * block called, and will not have its default command scheduled.
+ *
+ * @param subsystems the subsystem to un-register
+ */
+ public void unregisterSubsystem(Subsystem... subsystems) {
+ m_subsystems.keySet().removeAll(Set.of(subsystems));
+ }
+
+ /**
+ * Sets the default command for a subsystem. Registers that subsystem if it is not already
+ * registered. Default commands will run whenever there is no other command currently scheduled
+ * that requires the subsystem. Default commands should be written to never end (i.e. their
+ * {@link Command#isFinished()} method should return false), as they would simply be re-scheduled
+ * if they do. Default commands must also require their subsystem.
+ *
+ * @param subsystem the subsystem whose default command will be set
+ * @param defaultCommand the default command to associate with the subsystem
+ */
+ public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) {
+ if (!defaultCommand.getRequirements().contains(subsystem)) {
+ throw new IllegalArgumentException("Default commands must require their subsystem!");
+ }
+
+ if (defaultCommand.isFinished()) {
+ throw new IllegalArgumentException("Default commands should not end!");
+ }
+
+ m_subsystems.put(subsystem, defaultCommand);
+ }
+
+ /**
+ * Gets the default command associated with this subsystem. Null if this subsystem has no default
+ * command associated with it.
+ *
+ * @param subsystem the subsystem to inquire about
+ * @return the default command associated with the subsystem
+ */
+ public Command getDefaultCommand(Subsystem subsystem) {
+ return m_subsystems.get(subsystem);
+ }
+
+ /**
+ * Cancels commands. The scheduler will only call the interrupted method of a canceled command,
+ * not the end method (though the interrupted method may itself call the end method). Commands
+ * will be canceled even if they are not scheduled as interruptible.
+ *
+ * @param commands the commands to cancel
+ */
+ public void cancel(Command... commands) {
+ if (m_inRunLoop) {
+ m_toCancel.addAll(List.of(commands));
+ return;
+ }
+
+ for (Command command : commands) {
+ if (!m_scheduledCommands.containsKey(command)) {
+ continue;
+ }
+
+ command.end(true);
+ for (Consumer<Command> action : m_interruptActions) {
+ action.accept(command);
+ }
+ m_scheduledCommands.remove(command);
+ m_requirements.keySet().removeAll(command.getRequirements());
+ }
+ }
+
+ /**
+ * Cancels all commands that are currently scheduled.
+ */
+ public void cancelAll() {
+ for (Command command : m_scheduledCommands.keySet()) {
+ cancel(command);
+ }
+ }
+
+ /**
+ * Returns the time since a given command was scheduled. Note that this only works on commands
+ * that are directly scheduled by the scheduler; it will not work on commands inside of
+ * commandgroups, as the scheduler does not see them.
+ *
+ * @param command the command to query
+ * @return the time since the command was scheduled, in seconds
+ */
+ public double timeSinceScheduled(Command command) {
+ CommandState commandState = m_scheduledCommands.get(command);
+ if (commandState != null) {
+ return commandState.timeSinceInitialized();
+ } else {
+ return -1;
+ }
+ }
+
+ /**
+ * Whether the given commands are running. Note that this only works on commands that are
+ * directly scheduled by the scheduler; it will not work on commands inside of CommandGroups, as
+ * the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ public boolean isScheduled(Command... commands) {
+ return m_scheduledCommands.keySet().containsAll(Set.of(commands));
+ }
+
+ /**
+ * Returns the command currently requiring a given subsystem. Null if no command is currently
+ * requiring the subsystem
+ *
+ * @param subsystem the subsystem to be inquired about
+ * @return the command currently requiring the subsystem
+ */
+ public Command requiring(Subsystem subsystem) {
+ return m_requirements.get(subsystem);
+ }
+
+ /**
+ * Disables the command scheduler.
+ */
+ public void disable() {
+ m_disabled = true;
+ }
+
+ /**
+ * Enables the command scheduler.
+ */
+ public void enable() {
+ m_disabled = false;
+ }
+
+ /**
+ * Adds an action to perform on the initialization of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ public void onCommandInitialize(Consumer<Command> action) {
+ m_initActions.add(action);
+ }
+
+ /**
+ * Adds an action to perform on the execution of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ public void onCommandExecute(Consumer<Command> action) {
+ m_executeActions.add(action);
+ }
+
+ /**
+ * Adds an action to perform on the interruption of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ public void onCommandInterrupt(Consumer<Command> action) {
+ m_interruptActions.add(action);
+ }
+
+ /**
+ * Adds an action to perform on the finishing of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ public void onCommandFinish(Consumer<Command> action) {
+ m_finishActions.add(action);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Scheduler");
+ m_namesEntry = builder.getEntry("Names");
+ m_idsEntry = builder.getEntry("Ids");
+ m_cancelEntry = builder.getEntry("Cancel");
+ builder.setUpdateTable(() -> {
+
+ if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
+ return;
+ }
+
+ Map<Double, Command> ids = new LinkedHashMap<>();
+
+
+ for (Command command : m_scheduledCommands.keySet()) {
+ ids.put((double) command.hashCode(), command);
+ }
+
+ double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+ if (toCancel.length > 0) {
+ for (double hash : toCancel) {
+ cancel(ids.get(hash));
+ ids.remove(hash);
+ }
+ m_cancelEntry.setDoubleArray(new double[0]);
+ }
+
+ List<String> names = new ArrayList<>();
+
+ ids.values().forEach(command -> names.add(command.getName()));
+
+ m_namesEntry.setStringArray(names.toArray(new String[0]));
+ m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
+ });
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
new file mode 100644
index 0000000..2e2dc7e
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * Class that holds scheduling state for a command. Used internally by the
+ * {@link CommandScheduler}.
+ */
+class CommandState {
+ //The time since this command was initialized.
+ private double m_startTime = -1;
+
+ //Whether or not it is interruptible.
+ private final boolean m_interruptible;
+
+ CommandState(boolean interruptible) {
+ m_interruptible = interruptible;
+ startTiming();
+ startRunning();
+ }
+
+ private void startTiming() {
+ m_startTime = Timer.getFPGATimestamp();
+ }
+
+ synchronized void startRunning() {
+ m_startTime = -1;
+ }
+
+ boolean isInterruptible() {
+ return m_interruptible;
+ }
+
+ double timeSinceInitialized() {
+ return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
new file mode 100644
index 0000000..8e3239c
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+/**
+ * Runs one of two commands, depending on the value of the given condition when this command is
+ * initialized. Does not actually schedule the selected command - rather, the command is run
+ * through this command; this ensures that the command will behave as expected if used as part of a
+ * CommandGroup. Requires the requirements of both commands, again to ensure proper functioning
+ * when used in a CommandGroup. If this is undesired, consider using {@link ScheduleCommand}.
+ *
+ * <p>As this command contains multiple component commands within it, it is technically a command
+ * group; the command instances that are passed to it cannot be added to any other groups, or
+ * scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ConditionalCommand extends CommandBase {
+ private final Command m_onTrue;
+ private final Command m_onFalse;
+ private final BooleanSupplier m_condition;
+ private Command m_selectedCommand;
+
+ /**
+ * Creates a new ConditionalCommand.
+ *
+ * @param onTrue the command to run if the condition is true
+ * @param onFalse the command to run if the condition is false
+ * @param condition the condition to determine which command to run
+ */
+ public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) {
+ requireUngrouped(onTrue, onFalse);
+
+ CommandGroupBase.registerGroupedCommands(onTrue, onFalse);
+
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+ m_condition = requireNonNullParam(condition, "condition", "ConditionalCommand");
+ m_requirements.addAll(m_onTrue.getRequirements());
+ m_requirements.addAll(m_onFalse.getRequirements());
+ }
+
+ @Override
+ public void initialize() {
+ if (m_condition.getAsBoolean()) {
+ m_selectedCommand = m_onTrue;
+ } else {
+ m_selectedCommand = m_onFalse;
+ }
+ m_selectedCommand.initialize();
+ }
+
+ @Override
+ public void execute() {
+ m_selectedCommand.execute();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_selectedCommand.end(interrupted);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_selectedCommand.isFinished();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return m_onTrue.runsWhenDisabled() && m_onFalse.runsWhenDisabled();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
new file mode 100644
index 0000000..b9dd6db
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that allows the user to pass in functions for each of the basic command methods through
+ * the constructor. Useful for inline definitions of complex commands - note, however, that if a
+ * command is beyond a certain complexity it is usually better practice to write a proper class for
+ * it than to inline it.
+ */
+public class FunctionalCommand extends CommandBase {
+ protected final Runnable m_onInit;
+ protected final Runnable m_onExecute;
+ protected final Consumer<Boolean> m_onEnd;
+ protected final BooleanSupplier m_isFinished;
+
+ /**
+ * Creates a new FunctionalCommand.
+ *
+ * @param onInit the function to run on command initialization
+ * @param onExecute the function to run on command execution
+ * @param onEnd the function to run on command end
+ * @param isFinished the function that determines whether the command has finished
+ * @param requirements the subsystems required by this command
+ */
+ public FunctionalCommand(Runnable onInit, Runnable onExecute, Consumer<Boolean> onEnd,
+ BooleanSupplier isFinished, Subsystem... requirements) {
+ m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand");
+ m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand");
+ m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand");
+ m_isFinished = requireNonNullParam(isFinished, "isFinished", "FunctionalCommand");
+
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_onInit.run();
+ }
+
+ @Override
+ public void execute() {
+ m_onExecute.run();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_onEnd.accept(interrupted);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_isFinished.getAsBoolean();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
new file mode 100644
index 0000000..8518890
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A Command that runs instantly; it will initialize, execute once, and end on the same
+ * iteration of the scheduler. Users can either pass in a Runnable and a set of requirements,
+ * or else subclass this command if desired.
+ */
+public class InstantCommand extends CommandBase {
+ private final Runnable m_toRun;
+
+ /**
+ * Creates a new InstantCommand that runs the given Runnable with the given requirements.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems required by this command
+ */
+ public InstantCommand(Runnable toRun, Subsystem... requirements) {
+ m_toRun = requireNonNullParam(toRun, "toRun", "InstantCommand");
+
+ addRequirements(requirements);
+ }
+
+ /**
+ * Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg
+ * constructor to call implicitly from subclass constructors.
+ */
+ public InstantCommand() {
+ m_toRun = () -> {
+ };
+ }
+
+ @Override
+ public void initialize() {
+ m_toRun.run();
+ }
+
+ @Override
+ public final boolean isFinished() {
+ return true;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
new file mode 100644
index 0000000..af50dc8
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a mecanum drive.
+ *
+ * <p>The command handles trajectory-following,
+ * Velocity PID calculations, and feedforwards internally. This
+ * is intended to be a more-or-less "complete solution" that can be used by teams without a great
+ * deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
+ * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
+ * the PID and feedforward functionality, returning only the raw wheel speeds from the PID
+ * controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ */
+
+@SuppressWarnings({"PMD.TooManyFields", "MemberName"})
+public class MecanumControllerCommand extends CommandBase {
+ private final Timer m_timer = new Timer();
+ private MecanumDriveWheelSpeeds m_prevSpeeds;
+ private double m_prevTime;
+ private Pose2d m_finalPose;
+ private final boolean m_usePID;
+
+ private final Trajectory m_trajectory;
+ private final Supplier<Pose2d> m_pose;
+ private final SimpleMotorFeedforward m_feedforward;
+ private final MecanumDriveKinematics m_kinematics;
+ private final PIDController m_xController;
+ private final PIDController m_yController;
+ private final ProfiledPIDController m_thetaController;
+ private final double m_maxWheelVelocityMetersPerSecond;
+ private final PIDController m_frontLeftController;
+ private final PIDController m_rearLeftController;
+ private final PIDController m_frontRightController;
+ private final PIDController m_rearRightController;
+ private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
+ private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
+ private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
+
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow the provided
+ * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+ * 12 as a voltage output to the motor.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+ * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the final pose in the
+ * trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param feedforward The feedforward to use for the drivetrain.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+ * @param frontLeftController The front left wheel velocity PID.
+ * @param rearLeftController The rear left wheel velocity PID.
+ * @param frontRightController The front right wheel velocity PID.
+ * @param rearRightController The rear right wheel velocity PID.
+ * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
+ * the current wheel speeds.
+ * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing
+ * the output motor voltages.
+ * @param requirements The subsystems to require.
+ */
+
+ @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+ public MecanumControllerCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ SimpleMotorFeedforward feedforward,
+ MecanumDriveKinematics kinematics,
+
+ PIDController xController,
+ PIDController yController,
+ ProfiledPIDController thetaController,
+
+ double maxWheelVelocityMetersPerSecond,
+
+ PIDController frontLeftController,
+ PIDController rearLeftController,
+ PIDController frontRightController,
+ PIDController rearRightController,
+
+ Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+
+ Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+ m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+ m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
+ m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
+
+ m_xController = requireNonNullParam(xController, "xController",
+ "MecanumControllerCommand");
+ m_yController = requireNonNullParam(yController, "yController",
+ "MecanumControllerCommand");
+ m_thetaController = requireNonNullParam(thetaController, "thetaController",
+ "MecanumControllerCommand");
+
+ m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+ "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+ m_frontLeftController = requireNonNullParam(frontLeftController,
+ "frontLeftController", "MecanumControllerCommand");
+ m_rearLeftController = requireNonNullParam(rearLeftController,
+ "rearLeftController", "MecanumControllerCommand");
+ m_frontRightController = requireNonNullParam(frontRightController,
+ "frontRightController", "MecanumControllerCommand");
+ m_rearRightController = requireNonNullParam(rearRightController,
+ "rearRightController", "MecanumControllerCommand");
+
+ m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
+ "currentWheelSpeeds", "MecanumControllerCommand");
+
+ m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
+ "outputDriveVoltages", "MecanumControllerCommand");
+
+ m_outputWheelSpeeds = null;
+
+ m_usePID = true;
+
+ addRequirements(requirements);
+ }
+
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow the provided
+ * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
+ * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
+ *
+ * <p>Note2: The rotation controller will calculate the rotation based on the final pose
+ * in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+ * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing
+ * the output wheel speeds.
+ * @param requirements The subsystems to require.
+ */
+
+ @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+ public MecanumControllerCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ MecanumDriveKinematics kinematics,
+ PIDController xController,
+ PIDController yController,
+ ProfiledPIDController thetaController,
+
+ double maxWheelVelocityMetersPerSecond,
+
+ Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+ m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+ m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+ m_kinematics = requireNonNullParam(kinematics,
+ "kinematics", "MecanumControllerCommand");
+
+ m_xController = requireNonNullParam(xController,
+ "xController", "MecanumControllerCommand");
+ m_yController = requireNonNullParam(yController,
+ "xController", "MecanumControllerCommand");
+ m_thetaController = requireNonNullParam(thetaController,
+ "thetaController", "MecanumControllerCommand");
+
+ m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+ "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+ m_frontLeftController = null;
+ m_rearLeftController = null;
+ m_frontRightController = null;
+ m_rearRightController = null;
+
+ m_currentWheelSpeeds = null;
+
+ m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
+ "outputWheelSpeeds", "MecanumControllerCommand");
+
+ m_outputDriveVoltages = null;
+
+ m_usePID = false;
+
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ var initialState = m_trajectory.sample(0);
+
+ // Sample final pose to get robot rotation
+ m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
+
+ var initialXVelocity = initialState.velocityMetersPerSecond
+ * initialState.poseMeters.getRotation().getCos();
+ var initialYVelocity = initialState.velocityMetersPerSecond
+ * initialState.poseMeters.getRotation().getSin();
+
+ m_prevSpeeds = m_kinematics.toWheelSpeeds(
+ new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
+
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ @Override
+ @SuppressWarnings("LocalVariableName")
+ public void execute() {
+ double curTime = m_timer.get();
+ double dt = curTime - m_prevTime;
+
+ var desiredState = m_trajectory.sample(curTime);
+ var desiredPose = desiredState.poseMeters;
+
+ var poseError = desiredPose.relativeTo(m_pose.get());
+
+ double targetXVel = m_xController.calculate(
+ m_pose.get().getTranslation().getX(),
+ desiredPose.getTranslation().getX());
+
+ double targetYVel = m_yController.calculate(
+ m_pose.get().getTranslation().getY(),
+ desiredPose.getTranslation().getY());
+
+ // The robot will go to the desired rotation of the final pose in the trajectory,
+ // not following the poses at individual states.
+ double targetAngularVel = m_thetaController.calculate(
+ m_pose.get().getRotation().getRadians(),
+ m_finalPose.getRotation().getRadians());
+
+ double vRef = desiredState.velocityMetersPerSecond;
+
+ targetXVel += vRef * poseError.getRotation().getCos();
+ targetYVel += vRef * poseError.getRotation().getSin();
+
+ var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
+
+ var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
+
+ targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
+
+ var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
+ var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
+ var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
+ var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
+
+ double frontLeftOutput;
+ double rearLeftOutput;
+ double frontRightOutput;
+ double rearRightOutput;
+
+ if (m_usePID) {
+ final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
+ (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
+
+ final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
+ (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
+
+ final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
+ (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
+
+ final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
+ (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
+
+ frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
+ m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
+ frontLeftSpeedSetpoint);
+
+ rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
+ m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
+ rearLeftSpeedSetpoint);
+
+ frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
+ m_currentWheelSpeeds.get().frontRightMetersPerSecond,
+ frontRightSpeedSetpoint);
+
+ rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
+ m_currentWheelSpeeds.get().rearRightMetersPerSecond,
+ rearRightSpeedSetpoint);
+
+ m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
+ frontLeftOutput,
+ frontRightOutput,
+ rearLeftOutput,
+ rearRightOutput));
+
+ } else {
+ m_outputWheelSpeeds.accept(new MecanumDriveWheelSpeeds(
+ frontLeftSpeedSetpoint,
+ frontRightSpeedSetpoint,
+ rearLeftSpeedSetpoint,
+ rearRightSpeedSetpoint));
+ }
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
new file mode 100644
index 0000000..e63fcc5
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.Notifier;
+
+/**
+ * A command that starts a notifier to run the given runnable periodically in a separate thread.
+ * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
+ * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ *
+ * <p>WARNING: Do not use this class unless you are confident in your ability to make the executed
+ * code thread-safe. If you do not know what "thread-safe" means, that is a good sign that
+ * you should not use this class.
+ */
+public class NotifierCommand extends CommandBase {
+ protected final Notifier m_notifier;
+ protected final double m_period;
+
+ /**
+ * Creates a new NotifierCommand.
+ *
+ * @param toRun the runnable for the notifier to run
+ * @param period the period at which the notifier should run, in seconds
+ * @param requirements the subsystems required by this command
+ */
+ public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
+ m_notifier = new Notifier(toRun);
+ m_period = period;
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_notifier.startPeriodic(m_period);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_notifier.stop();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
new file mode 100644
index 0000000..5f2b093
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that controls an output with a {@link PIDController}. Runs forever by default - to add
+ * exit conditions and/or other behavior, subclass this class. The controller calculation and
+ * output are performed synchronously in the command's execute() method.
+ */
+public class PIDCommand extends CommandBase {
+ protected final PIDController m_controller;
+ protected DoubleSupplier m_measurement;
+ protected DoubleSupplier m_setpoint;
+ protected DoubleConsumer m_useOutput;
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a PIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpointSource the controller's setpoint
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
+ DoubleSupplier setpointSource, DoubleConsumer useOutput,
+ Subsystem... requirements) {
+ requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+ requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+ requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
+ requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+ m_controller = controller;
+ m_useOutput = useOutput;
+ m_measurement = measurementSource;
+ m_setpoint = setpointSource;
+ m_requirements.addAll(Set.of(requirements));
+ }
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a PIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpoint the controller's setpoint
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
+ double setpoint, DoubleConsumer useOutput,
+ Subsystem... requirements) {
+ this(controller, measurementSource, () -> setpoint, useOutput, requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_controller.reset();
+ }
+
+ @Override
+ public void execute() {
+ m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
+ m_setpoint.getAsDouble()));
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_useOutput.accept(0);
+ }
+
+ /**
+ * Returns the PIDController used by the command.
+ *
+ * @return The PIDController
+ */
+ public PIDController getController() {
+ return m_controller;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
new file mode 100644
index 0000000..575ebb4
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A subsystem that uses a {@link PIDController} to control an output. The controller is run
+ * synchronously from the subsystem's periodic() method.
+ */
+public abstract class PIDSubsystem extends SubsystemBase {
+ protected final PIDController m_controller;
+ protected boolean m_enabled;
+
+ private double m_setpoint;
+
+ /**
+ * Creates a new PIDSubsystem.
+ *
+ * @param controller the PIDController to use
+ */
+ public PIDSubsystem(PIDController controller) {
+ requireNonNullParam(controller, "controller", "PIDSubsystem");
+ m_controller = controller;
+ }
+
+ @Override
+ public void periodic() {
+ if (m_enabled) {
+ useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
+ }
+ }
+
+ public PIDController getController() {
+ return m_controller;
+ }
+
+ /**
+ * Sets the setpoint for the subsystem.
+ *
+ * @param setpoint the setpoint for the subsystem
+ */
+ public void setSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+ }
+
+ /**
+ * Uses the output from the PIDController.
+ *
+ * @param output the output of the PIDController
+ * @param setpoint the setpoint of the PIDController (for feedforward)
+ */
+ protected abstract void useOutput(double output, double setpoint);
+
+ /**
+ * Returns the measurement of the process variable used by the PIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ protected abstract double getMeasurement();
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ public void enable() {
+ m_enabled = true;
+ m_controller.reset();
+ }
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ public void disable() {
+ m_enabled = false;
+ useOutput(0, 0);
+ }
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ public boolean isEnabled() {
+ return m_enabled;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
new file mode 100644
index 0000000..38cc3c1
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when the last command ends.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ParallelCommandGroup extends CommandGroupBase {
+ //maps commands in this group to whether they are still running
+ private final Map<Command, Boolean> m_commands = new HashMap<>();
+ private boolean m_runWhenDisabled = true;
+
+ /**
+ * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously.
+ * The command group will finish when the last command finishes. If the CommandGroup is
+ * interrupted, only the commands that are still running will be interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ public ParallelCommandGroup(Command... commands) {
+ addCommands(commands);
+ }
+
+ @Override
+ public final void addCommands(Command... commands) {
+ requireUngrouped(commands);
+
+ if (m_commands.containsValue(true)) {
+ throw new IllegalStateException(
+ "Commands cannot be added to a CommandGroup while the group is running");
+ }
+
+ registerGroupedCommands(commands);
+
+ for (Command command : commands) {
+ if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
+ throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+ + "require the same subsystems");
+ }
+ m_commands.put(command, false);
+ m_requirements.addAll(command.getRequirements());
+ m_runWhenDisabled &= command.runsWhenDisabled();
+ }
+ }
+
+ @Override
+ public void initialize() {
+ for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+ commandRunning.getKey().initialize();
+ commandRunning.setValue(true);
+ }
+ }
+
+ @Override
+ public void execute() {
+ for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+ if (!commandRunning.getValue()) {
+ continue;
+ }
+ commandRunning.getKey().execute();
+ if (commandRunning.getKey().isFinished()) {
+ commandRunning.getKey().end(false);
+ commandRunning.setValue(false);
+ }
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (interrupted) {
+ for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+ if (commandRunning.getValue()) {
+ commandRunning.getKey().end(true);
+ }
+ }
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return !m_commands.values().contains(true);
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return m_runWhenDisabled;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
new file mode 100644
index 0000000..37be702
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending only when a specific command
+ * (the "deadline") ends, interrupting all other commands that are still running at that point.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ParallelDeadlineGroup extends CommandGroupBase {
+ //maps commands in this group to whether they are still running
+ private final Map<Command, Boolean> m_commands = new HashMap<>();
+ private boolean m_runWhenDisabled = true;
+ private boolean m_finished = true;
+ private Command m_deadline;
+
+ /**
+ * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
+ * executed simultaneously. The CommandGroup will finish when the deadline finishes,
+ * interrupting all other still-running commands. If the CommandGroup is interrupted, only
+ * the commands still running will be interrupted.
+ *
+ * @param deadline the command that determines when the group ends
+ * @param commands the commands to be executed
+ */
+ public ParallelDeadlineGroup(Command deadline, Command... commands) {
+ m_deadline = deadline;
+ addCommands(commands);
+ if (!m_commands.containsKey(deadline)) {
+ addCommands(deadline);
+ }
+ }
+
+ /**
+ * Sets the deadline to the given command. The deadline is added to the group if it is not
+ * already contained.
+ *
+ * @param deadline the command that determines when the group ends
+ */
+ public void setDeadline(Command deadline) {
+ if (!m_commands.containsKey(deadline)) {
+ addCommands(deadline);
+ }
+ m_deadline = deadline;
+ }
+
+ @Override
+ public final void addCommands(Command... commands) {
+ requireUngrouped(commands);
+
+ if (!m_finished) {
+ throw new IllegalStateException(
+ "Commands cannot be added to a CommandGroup while the group is running");
+ }
+
+ registerGroupedCommands(commands);
+
+ for (Command command : commands) {
+ if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
+ throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+ + "require the same subsystems");
+ }
+ m_commands.put(command, false);
+ m_requirements.addAll(command.getRequirements());
+ m_runWhenDisabled &= command.runsWhenDisabled();
+ }
+ }
+
+ @Override
+ public void initialize() {
+ for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+ commandRunning.getKey().initialize();
+ commandRunning.setValue(true);
+ }
+ m_finished = false;
+ }
+
+ @Override
+ public void execute() {
+ for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+ if (!commandRunning.getValue()) {
+ continue;
+ }
+ commandRunning.getKey().execute();
+ if (commandRunning.getKey().isFinished()) {
+ commandRunning.getKey().end(false);
+ commandRunning.setValue(false);
+ if (commandRunning.getKey() == m_deadline) {
+ m_finished = true;
+ }
+ }
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
+ if (commandRunning.getValue()) {
+ commandRunning.getKey().end(true);
+ }
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_finished;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return m_runWhenDisabled;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
new file mode 100644
index 0000000..dcaf5f5
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Collections;
+import java.util.HashSet;
+import java.util.Set;
+
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when any one of the commands ends
+ * and interrupting all the others.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class ParallelRaceGroup extends CommandGroupBase {
+ private final Set<Command> m_commands = new HashSet<>();
+ private boolean m_runWhenDisabled = true;
+ private boolean m_finished = true;
+
+ /**
+ * Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and
+ * will "race to the finish" - the first command to finish ends the entire command, with all other
+ * commands being interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ public ParallelRaceGroup(Command... commands) {
+ addCommands(commands);
+ }
+
+ @Override
+ public final void addCommands(Command... commands) {
+ requireUngrouped(commands);
+
+ if (!m_finished) {
+ throw new IllegalStateException(
+ "Commands cannot be added to a CommandGroup while the group is running");
+ }
+
+ registerGroupedCommands(commands);
+
+ for (Command command : commands) {
+ if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
+ throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
+ + " require the same subsystems");
+ }
+ m_commands.add(command);
+ m_requirements.addAll(command.getRequirements());
+ m_runWhenDisabled &= command.runsWhenDisabled();
+ }
+ }
+
+ @Override
+ public void initialize() {
+ m_finished = false;
+ for (Command command : m_commands) {
+ command.initialize();
+ }
+ }
+
+ @Override
+ public void execute() {
+ for (Command command : m_commands) {
+ command.execute();
+ if (command.isFinished()) {
+ m_finished = true;
+ }
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ for (Command command : m_commands) {
+ command.end(!command.isFinished());
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_finished;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return m_runWhenDisabled;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
new file mode 100644
index 0000000..6ebb376
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+/**
+ * A command that runs another command in perpetuity, ignoring that command's end conditions. While
+ * this class does not extend {@link CommandGroupBase}, it is still considered a CommandGroup, as it
+ * allows one to compose another command within it; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class PerpetualCommand extends CommandBase {
+ protected final Command m_command;
+
+ /**
+ * Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that
+ * command's end conditions, unless this command itself is interrupted.
+ *
+ * @param command the command to run perpetually
+ */
+ public PerpetualCommand(Command command) {
+ requireUngrouped(command);
+ registerGroupedCommands(command);
+ m_command = command;
+ m_requirements.addAll(command.getRequirements());
+ }
+
+ @Override
+ public void initialize() {
+ m_command.initialize();
+ }
+
+ @Override
+ public void execute() {
+ m_command.execute();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_command.end(interrupted);
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return m_command.runsWhenDisabled();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
new file mode 100644
index 0000000..4fb4126
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+/**
+ * A command that prints a string when initialized.
+ */
+public class PrintCommand extends InstantCommand {
+ /**
+ * Creates a new a PrintCommand.
+ *
+ * @param message the message to print
+ */
+ public PrintCommand(String message) {
+ super(() -> System.out.println(message));
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
new file mode 100644
index 0000000..076b4b8
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.BiConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that controls an output with a {@link ProfiledPIDController}. Runs forever by
+ * default - to add
+ * exit conditions and/or other behavior, subclass this class. The controller calculation and
+ * output are performed synchronously in the command's execute() method.
+ */
+public class ProfiledPIDCommand extends CommandBase {
+ protected final ProfiledPIDController m_controller;
+ protected DoubleSupplier m_measurement;
+ protected Supplier<State> m_goal;
+ protected BiConsumer<Double, State> m_useOutput;
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
+ * Goal velocity is specified.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+ Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
+ Subsystem... requirements) {
+ requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+ requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+ requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
+ requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+ m_controller = controller;
+ m_useOutput = useOutput;
+ m_measurement = measurementSource;
+ m_goal = goalSource;
+ m_requirements.addAll(Set.of(requirements));
+ }
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
+ * Goal velocity is implicitly zero.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+ DoubleSupplier goalSource, BiConsumer<Double, State> useOutput,
+ Subsystem... requirements) {
+ requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+ requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+ requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
+ requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+ m_controller = controller;
+ m_useOutput = useOutput;
+ m_measurement = measurementSource;
+ m_goal = () -> new State(goalSource.getAsDouble(), 0);
+ m_requirements.addAll(Set.of(requirements));
+ }
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+ * velocity is specified.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+ State goal, BiConsumer<Double, State> useOutput,
+ Subsystem... requirements) {
+ this(controller, measurementSource, () -> goal, useOutput, requirements);
+ }
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+ * velocity is implicitly zero.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+ double goal, BiConsumer<Double, State> useOutput,
+ Subsystem... requirements) {
+ this(controller, measurementSource, () -> goal, useOutput, requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_controller.reset();
+ }
+
+ @Override
+ public void execute() {
+ m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
+ m_controller.getSetpoint());
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_useOutput.accept(0.0, new State());
+ }
+
+ /**
+ * Returns the ProfiledPIDController used by the command.
+ *
+ * @return The ProfiledPIDController
+ */
+ public ProfiledPIDController getController() {
+ return m_controller;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
new file mode 100644
index 0000000..6b3492d
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is
+ * run synchronously from the subsystem's periodic() method.
+ */
+public abstract class ProfiledPIDSubsystem extends SubsystemBase {
+ protected final ProfiledPIDController m_controller;
+ protected boolean m_enabled;
+
+ private TrapezoidProfile.State m_goal;
+
+ /**
+ * Creates a new ProfiledPIDSubsystem.
+ *
+ * @param controller the ProfiledPIDController to use
+ */
+ public ProfiledPIDSubsystem(ProfiledPIDController controller) {
+ requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+ m_controller = controller;
+ }
+
+ @Override
+ public void periodic() {
+ if (m_enabled) {
+ useOutput(m_controller.calculate(getMeasurement(), m_goal), m_controller.getSetpoint());
+ }
+ }
+
+ public ProfiledPIDController getController() {
+ return m_controller;
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ public void setGoal(TrapezoidProfile.State goal) {
+ m_goal = goal;
+ }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ public void setGoal(double goal) {
+ setGoal(new TrapezoidProfile.State(goal, 0));
+ }
+
+ /**
+ * Uses the output from the ProfiledPIDController.
+ *
+ * @param output the output of the ProfiledPIDController
+ * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
+ */
+ protected abstract void useOutput(double output, State setpoint);
+
+ /**
+ * Returns the measurement of the process variable used by the ProfiledPIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ protected abstract double getMeasurement();
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ public void enable() {
+ m_enabled = true;
+ m_controller.reset();
+ }
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ public void disable() {
+ m_enabled = false;
+ useOutput(0, new State());
+ }
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ public boolean isEnabled() {
+ return m_enabled;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
new file mode 100644
index 0000000..27d67bc
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+/**
+ * Schedules the given commands when this command is initialized, and ends when all the commands are
+ * no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted,
+ * it will cancel all of the commands.
+ */
+public class ProxyScheduleCommand extends CommandBase {
+ private final Set<Command> m_toSchedule;
+ private boolean m_finished;
+
+ /**
+ * Creates a new ProxyScheduleCommand that schedules the given commands when initialized,
+ * and ends when they are all no longer scheduled.
+ *
+ * @param toSchedule the commands to schedule
+ */
+ public ProxyScheduleCommand(Command... toSchedule) {
+ m_toSchedule = Set.of(toSchedule);
+ }
+
+ @Override
+ public void initialize() {
+ for (Command command : m_toSchedule) {
+ command.schedule();
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (interrupted) {
+ for (Command command : m_toSchedule) {
+ command.cancel();
+ }
+ }
+ }
+
+ @Override
+ public void execute() {
+ m_finished = true;
+ for (Command command : m_toSchedule) {
+ m_finished &= !command.isScheduled();
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_finished;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
new file mode 100644
index 0000000..eea06c7
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -0,0 +1,206 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BiConsumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
+ * {@link Trajectory} with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and feedforwards internally. This
+ * is intended to be a more-or-less "complete solution" that can be used by teams without a great
+ * deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
+ * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
+ * the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE
+ * controller.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class RamseteCommand extends CommandBase {
+ private final Timer m_timer = new Timer();
+ private final boolean m_usePID;
+ private final Trajectory m_trajectory;
+ private final Supplier<Pose2d> m_pose;
+ private final RamseteController m_follower;
+ private final SimpleMotorFeedforward m_feedforward;
+ private final DifferentialDriveKinematics m_kinematics;
+ private final Supplier<DifferentialDriveWheelSpeeds> m_speeds;
+ private final PIDController m_leftController;
+ private final PIDController m_rightController;
+ private final BiConsumer<Double, Double> m_output;
+ private DifferentialDriveWheelSpeeds m_prevSpeeds;
+ private double m_prevTime;
+
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+ * PID control and feedforward are handled internally, and outputs are scaled -12 to 12
+ * representing units of volts.
+ *
+ * <p>Note: The controller will *not* set the outputVolts to zero upon completion of the path -
+ * this
+ * is left to the user, since it is not appropriate for paths with nonstationary endstates.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param controller The RAMSETE controller used to follow the trajectory.
+ * @param feedforward The feedforward to use for the drive.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param wheelSpeeds A function that supplies the speeds of the left and
+ * right sides of the robot drive.
+ * @param leftController The PIDController for the left side of the robot drive.
+ * @param rightController The PIDController for the right side of the robot drive.
+ * @param outputVolts A function that consumes the computed left and right
+ * outputs (in volts) for the robot drive.
+ * @param requirements The subsystems to require.
+ */
+ @SuppressWarnings("PMD.ExcessiveParameterList")
+ public RamseteCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ RamseteController controller,
+ SimpleMotorFeedforward feedforward,
+ DifferentialDriveKinematics kinematics,
+ Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
+ PIDController leftController,
+ PIDController rightController,
+ BiConsumer<Double, Double> outputVolts,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
+ m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
+ m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
+ m_feedforward = feedforward;
+ m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+ m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "RamseteCommand");
+ m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
+ m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
+ m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
+
+ m_usePID = true;
+
+ addRequirements(requirements);
+ }
+
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+ * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds
+ * from the RAMSETE controller, and will need to be converted into a usable form by the user.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param follower The RAMSETE follower used to follow the trajectory.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param outputMetersPerSecond A function that consumes the computed left and right
+ * wheel speeds.
+ * @param requirements The subsystems to require.
+ */
+ public RamseteCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ RamseteController follower,
+ DifferentialDriveKinematics kinematics,
+ BiConsumer<Double, Double> outputMetersPerSecond,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
+ m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
+ m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
+ m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+ m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
+
+ m_feedforward = null;
+ m_speeds = null;
+ m_leftController = null;
+ m_rightController = null;
+
+ m_usePID = false;
+
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_prevTime = 0;
+ var initialState = m_trajectory.sample(0);
+ m_prevSpeeds = m_kinematics.toWheelSpeeds(
+ new ChassisSpeeds(initialState.velocityMetersPerSecond,
+ 0,
+ initialState.curvatureRadPerMeter
+ * initialState.velocityMetersPerSecond));
+ m_timer.reset();
+ m_timer.start();
+ if (m_usePID) {
+ m_leftController.reset();
+ m_rightController.reset();
+ }
+ }
+
+ @Override
+ public void execute() {
+ double curTime = m_timer.get();
+ double dt = curTime - m_prevTime;
+
+ var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
+ m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
+
+ var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
+ var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
+
+ double leftOutput;
+ double rightOutput;
+
+ if (m_usePID) {
+ double leftFeedforward =
+ m_feedforward.calculate(leftSpeedSetpoint,
+ (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
+
+ double rightFeedforward =
+ m_feedforward.calculate(rightSpeedSetpoint,
+ (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
+
+ leftOutput = leftFeedforward
+ + m_leftController.calculate(m_speeds.get().leftMetersPerSecond,
+ leftSpeedSetpoint);
+
+ rightOutput = rightFeedforward
+ + m_rightController.calculate(m_speeds.get().rightMetersPerSecond,
+ rightSpeedSetpoint);
+ } else {
+ leftOutput = leftSpeedSetpoint;
+ rightOutput = rightSpeedSetpoint;
+ }
+
+ m_output.accept(leftOutput, rightOutput);
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
new file mode 100644
index 0000000..9099be0
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a Runnable continuously. Has no end condition as-is;
+ * either subclass it or use {@link Command#withTimeout(double)} or
+ * {@link Command#withInterrupt(BooleanSupplier)} to give it one. If you only wish
+ * to execute a Runnable once, use {@link InstantCommand}.
+ */
+public class RunCommand extends CommandBase {
+ protected final Runnable m_toRun;
+
+ /**
+ * Creates a new RunCommand. The Runnable will be run continuously until the command
+ * ends. Does not run when disabled.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems to require
+ */
+ public RunCommand(Runnable toRun, Subsystem... requirements) {
+ m_toRun = requireNonNullParam(toRun, "toRun", "RunCommand");
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void execute() {
+ m_toRun.run();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
new file mode 100644
index 0000000..700925b
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+/**
+ * Schedules the given commands when this command is initialized. Useful for forking off from
+ * CommandGroups. Note that if run from a CommandGroup, the group will not know about the status
+ * of the scheduled commands, and will treat this command as finishing instantly.
+ */
+public class ScheduleCommand extends CommandBase {
+ private final Set<Command> m_toSchedule;
+
+ /**
+ * Creates a new ScheduleCommand that schedules the given commands when initialized.
+ *
+ * @param toSchedule the commands to schedule
+ */
+ public ScheduleCommand(Command... toSchedule) {
+ m_toSchedule = Set.of(toSchedule);
+ }
+
+ @Override
+ public void initialize() {
+ for (Command command : m_toSchedule) {
+ command.schedule();
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return true;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
new file mode 100644
index 0000000..92fe07f
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+/**
+ * Runs one of a selection of commands, either using a selector and a key to command mapping, or a
+ * supplier that returns the command directly at runtime. Does not actually schedule the selected
+ * command - rather, the command is run through this command; this ensures that the command will
+ * behave as expected if used as part of a CommandGroup. Requires the requirements of all included
+ * commands, again to ensure proper functioning when used in a CommandGroup. If this is undesired,
+ * consider using {@link ScheduleCommand}.
+ *
+ * <p>As this command contains multiple component commands within it, it is technically a command
+ * group; the command instances that are passed to it cannot be added to any other groups, or
+ * scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class SelectCommand extends CommandBase {
+ private final Map<Object, Command> m_commands;
+ private final Supplier<Object> m_selector;
+ private final Supplier<Command> m_toRun;
+ private Command m_selectedCommand;
+
+ /**
+ * Creates a new selectcommand.
+ *
+ * @param commands the map of commands to choose from
+ * @param selector the selector to determine which command to run
+ */
+ public SelectCommand(Map<Object, Command> commands, Supplier<Object> selector) {
+ requireUngrouped(commands.values());
+
+ CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{}));
+
+ m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
+ m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
+
+ m_toRun = null;
+
+ for (Command command : m_commands.values()) {
+ m_requirements.addAll(command.getRequirements());
+ }
+ }
+
+ /**
+ * Creates a new selectcommand.
+ *
+ * @param toRun a supplier providing the command to run
+ */
+ public SelectCommand(Supplier<Command> toRun) {
+ m_commands = null;
+ m_selector = null;
+ m_toRun = requireNonNullParam(toRun, "toRun", "SelectCommand");
+ }
+
+ @Override
+ public void initialize() {
+ if (m_selector != null) {
+ if (!m_commands.keySet().contains(m_selector.get())) {
+ m_selectedCommand = new PrintCommand(
+ "SelectCommand selector value does not correspond to" + " any command!");
+ return;
+ }
+ m_selectedCommand = m_commands.get(m_selector.get());
+ } else {
+ m_selectedCommand = m_toRun.get();
+ }
+ m_selectedCommand.initialize();
+ }
+
+ @Override
+ public void execute() {
+ m_selectedCommand.execute();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_selectedCommand.end(interrupted);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_selectedCommand.isFinished();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ if (m_commands != null) {
+ boolean runsWhenDisabled = true;
+ for (Command command : m_commands.values()) {
+ runsWhenDisabled &= command.runsWhenDisabled();
+ }
+ return runsWhenDisabled;
+ } else {
+ return m_toRun.get().runsWhenDisabled();
+ }
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
new file mode 100644
index 0000000..0d01f11
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * A CommandGroups that runs a list of commands in sequence.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
+ */
+public class SequentialCommandGroup extends CommandGroupBase {
+ private final List<Command> m_commands = new ArrayList<>();
+ private int m_currentCommandIndex = -1;
+ private boolean m_runWhenDisabled = true;
+
+ /**
+ * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with
+ * the CommandGroup finishing when the last command finishes.
+ *
+ * @param commands the commands to include in this group.
+ */
+ public SequentialCommandGroup(Command... commands) {
+ addCommands(commands);
+ }
+
+ @Override
+ public final void addCommands(Command... commands) {
+ requireUngrouped(commands);
+
+ if (m_currentCommandIndex != -1) {
+ throw new IllegalStateException(
+ "Commands cannot be added to a CommandGroup while the group is running");
+ }
+
+ registerGroupedCommands(commands);
+
+ for (Command command : commands) {
+ m_commands.add(command);
+ m_requirements.addAll(command.getRequirements());
+ m_runWhenDisabled &= command.runsWhenDisabled();
+ }
+ }
+
+ @Override
+ public void initialize() {
+ m_currentCommandIndex = 0;
+
+ if (!m_commands.isEmpty()) {
+ m_commands.get(0).initialize();
+ }
+ }
+
+ @Override
+ public void execute() {
+ if (m_commands.isEmpty()) {
+ return;
+ }
+
+ Command currentCommand = m_commands.get(m_currentCommandIndex);
+
+ currentCommand.execute();
+ if (currentCommand.isFinished()) {
+ currentCommand.end(false);
+ m_currentCommandIndex++;
+ if (m_currentCommandIndex < m_commands.size()) {
+ m_commands.get(m_currentCommandIndex).initialize();
+ }
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1
+ && m_currentCommandIndex < m_commands.size()
+ ) {
+ m_commands.get(m_currentCommandIndex).end(true);
+ }
+ m_currentCommandIndex = -1;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_currentCommandIndex == m_commands.size();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return m_runWhenDisabled;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
new file mode 100644
index 0000000..3139465
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BooleanSupplier;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a given runnable when it is initalized, and another runnable when it ends.
+ * Useful for running and then stopping a motor, or extending and then retracting a solenoid.
+ * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
+ * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ */
+public class StartEndCommand extends CommandBase {
+ protected final Runnable m_onInit;
+ protected final Runnable m_onEnd;
+
+ /**
+ * Creates a new StartEndCommand. Will run the given runnables when the command starts and when
+ * it ends.
+ *
+ * @param onInit the Runnable to run on command init
+ * @param onEnd the Runnable to run on command end
+ * @param requirements the subsystems required by this command
+ */
+ public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) {
+ m_onInit = requireNonNullParam(onInit, "onInit", "StartEndCommand");
+ m_onEnd = requireNonNullParam(onEnd, "onEnd", "StartEndCommand");
+
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_onInit.run();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_onEnd.run();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
new file mode 100644
index 0000000..7dc917b
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+/**
+ * A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
+ * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc) and
+ * provide methods through which they can be used by {@link Command}s. Subsystems are used by the
+ * {@link CommandScheduler}'s resource management system to ensure multiple robot actions are not
+ * "fighting" over the same hardware; Commands that use a subsystem should include that subsystem
+ * in their {@link Command#getRequirements()} method, and resources used within a subsystem should
+ * generally remain encapsulated and not be shared by other parts of the robot.
+ *
+ * <p>Subsystems must be registered with the scheduler with the
+ * {@link CommandScheduler#registerSubsystem(Subsystem...)} method in order for the
+ * {@link Subsystem#periodic()} method to be called. It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations. The {@link SubsystemBase}
+ * class offers a simple base for user implementations that handles this.
+ */
+public interface Subsystem {
+
+ /**
+ * This method is called periodically by the {@link CommandScheduler}. Useful for updating
+ * subsystem-specific state that you don't want to offload to a {@link Command}. Teams should
+ * try to be consistent within their own codebases about which responsibilities will be handled
+ * by Commands, and which will be handled here.
+ */
+ default void periodic() {
+ }
+
+ /**
+ * Sets the default {@link Command} of the subsystem. The default command will be
+ * automatically scheduled when no other commands are scheduled that require the subsystem.
+ * Default commands should generally not end on their own, i.e. their {@link Command#isFinished()}
+ * method should always return false. Will automatically register this subsystem with the
+ * {@link CommandScheduler}.
+ *
+ * @param defaultCommand the default command to associate with this subsystem
+ */
+ default void setDefaultCommand(Command defaultCommand) {
+ CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand);
+ }
+
+ /**
+ * Gets the default command for this subsystem. Returns null if no default command is
+ * currently associated with the subsystem.
+ *
+ * @return the default command associated with this subsystem
+ */
+ default Command getDefaultCommand() {
+ return CommandScheduler.getInstance().getDefaultCommand(this);
+ }
+
+ /**
+ * Returns the command currently running on this subsystem. Returns null if no command is
+ * currently scheduled that requires this subsystem.
+ *
+ * @return the scheduled command currently requiring this subsystem
+ */
+ default Command getCurrentCommand() {
+ return CommandScheduler.getInstance().requiring(this);
+ }
+
+ /**
+ * Registers this subsystem with the {@link CommandScheduler}, allowing its
+ * {@link Subsystem#periodic()} method to be called when the scheduler runs.
+ */
+ default void register() {
+ CommandScheduler.getInstance().registerSubsystem(this);
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
new file mode 100644
index 0000000..9ae8ae2
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A base for subsystems that handles registration in the constructor, and provides a more intuitive
+ * method for setting the default command.
+ */
+public abstract class SubsystemBase implements Subsystem, Sendable {
+
+ /**
+ * Constructor.
+ */
+ public SubsystemBase() {
+ String name = this.getClass().getSimpleName();
+ name = name.substring(name.lastIndexOf('.') + 1);
+ SendableRegistry.addLW(this, name, name);
+ CommandScheduler.getInstance().registerSubsystem(this);
+ }
+
+ /**
+ * Gets the name of this Subsystem.
+ *
+ * @return Name
+ */
+ @Override
+ public String getName() {
+ return SendableRegistry.getName(this);
+ }
+
+ /**
+ * Sets the name of this Subsystem.
+ *
+ * @param name name
+ */
+ @Override
+ public void setName(String name) {
+ SendableRegistry.setName(this, name);
+ }
+
+ /**
+ * Gets the subsystem name of this Subsystem.
+ *
+ * @return Subsystem name
+ */
+ @Override
+ public String getSubsystem() {
+ return SendableRegistry.getSubsystem(this);
+ }
+
+ /**
+ * Sets the subsystem name of this Subsystem.
+ *
+ * @param subsystem subsystem name
+ */
+ @Override
+ public void setSubsystem(String subsystem) {
+ SendableRegistry.setSubsystem(this, subsystem);
+ }
+
+ /**
+ * Associates a {@link Sendable} with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ public void addChild(String name, Sendable child) {
+ SendableRegistry.addLW(child, getSubsystem(), name);
+ SendableRegistry.addChild(this, child);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("Subsystem");
+
+ builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
+ builder.addStringProperty(".default",
+ () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", null);
+ builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
+ builder.addStringProperty(".command",
+ () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none", null);
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
new file mode 100644
index 0000000..b05dc20
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a swerve drive.
+ *
+ * <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState})
+ * in an array. The desired wheel and module rotation velocities should be taken
+ * from those and used in velocity PIDs.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ */
+
+@SuppressWarnings("MemberName")
+public class SwerveControllerCommand extends CommandBase {
+ private final Timer m_timer = new Timer();
+ private Pose2d m_finalPose;
+
+ private final Trajectory m_trajectory;
+ private final Supplier<Pose2d> m_pose;
+ private final SwerveDriveKinematics m_kinematics;
+ private final PIDController m_xController;
+ private final PIDController m_yController;
+ private final ProfiledPIDController m_thetaController;
+ private final Consumer<SwerveModuleState[]> m_outputModuleStates;
+
+ /**
+ * Constructs a new SwerveControllerCommand that when executed will follow the provided
+ * trajectory. This command will not return output voltages but rather raw module states from the
+ * position controllers which need to be put into a velocity PID.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
+ * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the final pose
+ * in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param outputModuleStates The raw output module states from the
+ * position controllers.
+ * @param requirements The subsystems to require.
+ */
+
+ @SuppressWarnings("ParameterName")
+ public SwerveControllerCommand(Trajectory trajectory,
+ Supplier<Pose2d> pose,
+ SwerveDriveKinematics kinematics,
+ PIDController xController,
+ PIDController yController,
+ ProfiledPIDController thetaController,
+
+ Consumer<SwerveModuleState[]> outputModuleStates,
+ Subsystem... requirements) {
+ m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
+ m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
+ m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
+
+ m_xController = requireNonNullParam(xController,
+ "xController", "SwerveControllerCommand");
+ m_yController = requireNonNullParam(yController,
+ "xController", "SwerveControllerCommand");
+ m_thetaController = requireNonNullParam(thetaController,
+ "thetaController", "SwerveControllerCommand");
+
+ m_outputModuleStates = requireNonNullParam(outputModuleStates,
+ "frontLeftOutput", "SwerveControllerCommand");
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ // Sample final pose to get robot rotation
+ m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
+
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ @Override
+ @SuppressWarnings("LocalVariableName")
+ public void execute() {
+ double curTime = m_timer.get();
+
+ var desiredState = m_trajectory.sample(curTime);
+ var desiredPose = desiredState.poseMeters;
+
+ var poseError = desiredPose.relativeTo(m_pose.get());
+
+ double targetXVel = m_xController.calculate(
+ m_pose.get().getTranslation().getX(),
+ desiredPose.getTranslation().getX());
+
+ double targetYVel = m_yController.calculate(
+ m_pose.get().getTranslation().getY(),
+ desiredPose.getTranslation().getY());
+
+ // The robot will go to the desired rotation of the final pose in the trajectory,
+ // not following the poses at individual states.
+ double targetAngularVel = m_thetaController.calculate(
+ m_pose.get().getRotation().getRadians(),
+ m_finalPose.getRotation().getRadians());
+
+ double vRef = desiredState.velocityMetersPerSecond;
+
+ targetXVel += vRef * poseError.getRotation().getCos();
+ targetYVel += vRef * poseError.getRotation().getSin();
+
+ var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
+
+ var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
+
+ m_outputModuleStates.accept(targetModuleStates);
+
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
new file mode 100644
index 0000000..85ddd03
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.Consumer;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism
+ * motion.
+ */
+public class TrapezoidProfileCommand extends CommandBase {
+ private final TrapezoidProfile m_profile;
+ private final Consumer<State> m_output;
+
+ private final Timer m_timer = new Timer();
+
+ /**
+ * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
+ * Output will be piped to the provided consumer function.
+ *
+ * @param profile The motion profile to execute.
+ * @param output The consumer for the profile output.
+ * @param requirements The subsystems required by this command.
+ */
+ public TrapezoidProfileCommand(TrapezoidProfile profile,
+ Consumer<State> output,
+ Subsystem... requirements) {
+ m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
+ m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
+ addRequirements(requirements);
+ }
+
+ @Override
+ public void initialize() {
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ @Override
+ public void execute() {
+ m_output.accept(m_profile.calculate(m_timer.get()));
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_profile.totalTime());
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
new file mode 100644
index 0000000..b7ec8c1
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * A subsystem that generates and runs trapezoidal motion profiles automatically. The user
+ * specifies how to use the current state of the motion profile by overriding the `useState` method.
+ */
+public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
+ private final double m_period;
+ private final TrapezoidProfile.Constraints m_constraints;
+
+ private TrapezoidProfile.State m_state;
+ private TrapezoidProfile.State m_goal;
+
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+ * @param initialPosition The initial position of the controller mechanism when the subsystem
+ * is constructed.
+ */
+ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+ double initialPosition) {
+ m_constraints = constraints;
+ m_state = new TrapezoidProfile.State(initialPosition, 0);
+ m_period = 0.02;
+ }
+
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+ * @param initialPosition The initial position of the controller mechanism when the subsystem
+ * is constructed.
+ * @param period The period of the main robot loop, in seconds.
+ */
+ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+ double initialPosition,
+ double period) {
+ m_constraints = constraints;
+ m_state = new TrapezoidProfile.State(initialPosition, 0);
+ m_period = period;
+ }
+
+ @Override
+ public void periodic() {
+ var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
+ m_state = profile.calculate(m_period);
+ useState(m_state);
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ public void setGoal(TrapezoidProfile.State goal) {
+ m_goal = goal;
+ }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ public void setGoal(double goal) {
+ setGoal(new TrapezoidProfile.State(goal, 0));
+ }
+
+ /**
+ * Users should override this to consume the current state of the motion profile.
+ *
+ * @param state The current state of the motion profile.
+ */
+ protected abstract void useState(TrapezoidProfile.State state);
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
new file mode 100644
index 0000000..5e8ebc3
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+
+/**
+ * A command that does nothing but takes a specified amount of time to finish. Useful for
+ * CommandGroups. Can also be subclassed to make a command with an internal timer.
+ */
+public class WaitCommand extends CommandBase {
+ protected Timer m_timer = new Timer();
+ private final double m_duration;
+
+ /**
+ * Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
+ *
+ * @param seconds the time to wait, in seconds
+ */
+ public WaitCommand(double seconds) {
+ m_duration = seconds;
+ SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
+ }
+
+ @Override
+ public void initialize() {
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ m_timer.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_timer.hasPeriodPassed(m_duration);
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
new file mode 100644
index 0000000..5c55fff
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that does nothing but ends after a specified match time or condition. Useful for
+ * CommandGroups.
+ */
+public class WaitUntilCommand extends CommandBase {
+ private final BooleanSupplier m_condition;
+
+ /**
+ * Creates a new WaitUntilCommand that ends after a given condition becomes true.
+ *
+ * @param condition the condition to determine when to end
+ */
+ public WaitUntilCommand(BooleanSupplier condition) {
+ m_condition = requireNonNullParam(condition, "condition", "WaitUntilCommand");
+ }
+
+ /**
+ * Creates a new WaitUntilCommand that ends after a given match time.
+ *
+ * <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT
+ * guarantee that the time at which the action is performed will be judged to be legal by the
+ * referees. When in doubt, add a safety factor or time the action manually.
+ *
+ * @param time the match time after which to end, in seconds
+ */
+ public WaitUntilCommand(double time) {
+ this(() -> Timer.getMatchTime() - time > 0);
+ }
+
+ @Override
+ public boolean isFinished() {
+ return m_condition.getAsBoolean();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
new file mode 100644
index 0000000..b7f26ae
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger
+ * button of a joystick to a "score" command.
+ *
+ * <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
+ * operator interface as a common use case of the more generalized Trigger objects. This is a simple
+ * wrapper around Trigger with the method names renamed to fit the Button object use.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public abstract class Button extends Trigger {
+ /**
+ * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
+ * overridden).
+ */
+ public Button() {
+ }
+
+ /**
+ * Creates a new button with the given condition determining whether it is pressed.
+ *
+ * @param isPressed returns whether or not the trigger should be active
+ */
+ public Button(BooleanSupplier isPressed) {
+ super(isPressed);
+ }
+
+ /**
+ * Starts the given command whenever the button is newly pressed.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this button, so calls can be chained
+ */
+ public Button whenPressed(final Command command, boolean interruptible) {
+ whenActive(command, interruptible);
+ return this;
+ }
+
+ /**
+ * Starts the given command whenever the button is newly pressed. The command is set to be
+ * interruptible.
+ *
+ * @param command the command to start
+ * @return this button, so calls can be chained
+ */
+ public Button whenPressed(final Command command) {
+ whenActive(command);
+ return this;
+ }
+
+ /**
+ * Runs the given runnable whenever the button is newly pressed.
+ *
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
+ * @return this button, so calls can be chained
+ */
+ public Button whenPressed(final Runnable toRun, Subsystem... requirements) {
+ whenActive(toRun, requirements);
+ return this;
+ }
+
+ /**
+ * Constantly starts the given command while the button is held.
+ *
+ * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
+ * be canceled when the button is released.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this button, so calls can be chained
+ */
+ public Button whileHeld(final Command command, boolean interruptible) {
+ whileActiveContinuous(command, interruptible);
+ return this;
+ }
+
+ /**
+ * Constantly starts the given command while the button is held.
+ *
+ * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
+ * be canceled when the button is released. The command is set to be interruptible.
+ *
+ * @param command the command to start
+ * @return this button, so calls can be chained
+ */
+ public Button whileHeld(final Command command) {
+ whileActiveContinuous(command);
+ return this;
+ }
+
+ /**
+ * Constantly runs the given runnable while the button is held.
+ *
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
+ * @return this button, so calls can be chained
+ */
+ public Button whileHeld(final Runnable toRun, Subsystem... requirements) {
+ whileActiveContinuous(toRun, requirements);
+ return this;
+ }
+
+ /**
+ * Starts the given command when the button is first pressed, and cancels it when it is released,
+ * but does not start it again if it ends or is otherwise interrupted.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this button, so calls can be chained
+ */
+ public Button whenHeld(final Command command, boolean interruptible) {
+ whileActiveOnce(command, interruptible);
+ return this;
+ }
+
+ /**
+ * Starts the given command when the button is first pressed, and cancels it when it is released,
+ * but does not start it again if it ends or is otherwise interrupted. The command is set to be
+ * interruptible.
+ *
+ * @param command the command to start
+ * @return this button, so calls can be chained
+ */
+ public Button whenHeld(final Command command) {
+ whileActiveOnce(command, true);
+ return this;
+ }
+
+
+ /**
+ * Starts the command when the button is released.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this button, so calls can be chained
+ */
+ public Button whenReleased(final Command command, boolean interruptible) {
+ whenInactive(command, interruptible);
+ return this;
+ }
+
+ /**
+ * Starts the command when the button is released. The command is set to be interruptible.
+ *
+ * @param command the command to start
+ * @return this button, so calls can be chained
+ */
+ public Button whenReleased(final Command command) {
+ whenInactive(command);
+ return this;
+ }
+
+ /**
+ * Runs the given runnable when the button is released.
+ *
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
+ * @return this button, so calls can be chained
+ */
+ public Button whenReleased(final Runnable toRun, Subsystem... requirements) {
+ whenInactive(toRun, requirements);
+ return this;
+ }
+
+ /**
+ * Toggles the command whenever the button is pressed (on then off then on).
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ */
+ public Button toggleWhenPressed(final Command command, boolean interruptible) {
+ toggleWhenActive(command, interruptible);
+ return this;
+ }
+
+ /**
+ * Toggles the command whenever the button is pressed (on then off then on). The command is set
+ * to be interruptible.
+ *
+ * @param command the command to start
+ * @return this button, so calls can be chained
+ */
+ public Button toggleWhenPressed(final Command command) {
+ toggleWhenActive(command);
+ return this;
+ }
+
+ /**
+ * Cancels the command when the button is pressed.
+ *
+ * @param command the command to start
+ * @return this button, so calls can be chained
+ */
+ public Button cancelWhenPressed(final Command command) {
+ cancelWhenActive(command);
+ return this;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
new file mode 100644
index 0000000..3f74f48
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/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.wpilibj2.command.button;
+
+/**
+ * This class is intended to be used within a program. The programmer can manually set its value.
+ * Also includes a setting for whether or not it should invert its value.
+ */
+public class InternalButton extends Button {
+ private boolean m_pressed;
+ private boolean m_inverted;
+
+ /**
+ * Creates an InternalButton that is not inverted.
+ */
+ public InternalButton() {
+ this(false);
+ }
+
+ /**
+ * Creates an InternalButton which is inverted depending on the input.
+ *
+ * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
+ * when set to false.
+ */
+ public InternalButton(boolean inverted) {
+ m_pressed = m_inverted = inverted;
+ }
+
+ public void setInverted(boolean inverted) {
+ m_inverted = inverted;
+ }
+
+ public void setPressed(boolean pressed) {
+ m_pressed = pressed;
+ }
+
+ @Override
+ public boolean get() {
+ return m_pressed ^ m_inverted;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
new file mode 100644
index 0000000..918bb6a
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A {@link Button} that gets its state from a {@link GenericHID}.
+ */
+public class JoystickButton extends Button {
+ private final GenericHID m_joystick;
+ private final int m_buttonNumber;
+
+ /**
+ * Creates a joystick button for triggering commands.
+ *
+ * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick,
+ * etc)
+ * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
+ */
+ public JoystickButton(GenericHID joystick, int buttonNumber) {
+ requireNonNullParam(joystick, "joystick", "JoystickButton");
+
+ m_joystick = joystick;
+ m_buttonNumber = buttonNumber;
+ }
+
+ /**
+ * Gets the value of the joystick button.
+ *
+ * @return The value of the joystick button
+ */
+ @Override
+ public boolean get() {
+ return m_joystick.getRawButton(m_buttonNumber);
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
new file mode 100644
index 0000000..823b756
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A {@link Button} that gets its state from a POV on a {@link GenericHID}.
+ */
+public class POVButton extends Button {
+ private final GenericHID m_joystick;
+ private final int m_angle;
+ private final int m_povNumber;
+
+ /**
+ * Creates a POV button for triggering commands.
+ *
+ * @param joystick The GenericHID object that has the POV
+ * @param angle The desired angle in degrees (e.g. 90, 270)
+ * @param povNumber The POV number (see {@link GenericHID#getPOV(int)})
+ */
+ public POVButton(GenericHID joystick, int angle, int povNumber) {
+ requireNonNullParam(joystick, "joystick", "POVButton");
+
+ m_joystick = joystick;
+ m_angle = angle;
+ m_povNumber = povNumber;
+ }
+
+ /**
+ * Creates a POV button for triggering commands.
+ * By default, acts on POV 0
+ *
+ * @param joystick The GenericHID object that has the POV
+ * @param angle The desired angle (e.g. 90, 270)
+ */
+ public POVButton(GenericHID joystick, int angle) {
+ this(joystick, angle, 0);
+ }
+
+ /**
+ * Checks whether the current value of the POV is the target angle.
+ *
+ * @return Whether the value of the POV matches the target angle
+ */
+ @Override
+ public boolean get() {
+ return m_joystick.getPOV(m_povNumber) == m_angle;
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
new file mode 100644
index 0000000..5167e4a
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
@@ -0,0 +1,355 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger
+ * button of a joystick to a "score" command.
+ *
+ * <p>It is encouraged that teams write a subclass of Trigger if they want to have something
+ * unusual (for instance, if they want to react to the user holding a button while the robot is
+ * reading a certain sensor input). For this, they only have to write the {@link Trigger#get()}
+ * method to get the full functionality of the Trigger class.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class Trigger {
+ private final BooleanSupplier m_isActive;
+
+ /**
+ * Creates a new trigger with the given condition determining whether it is active.
+ *
+ * @param isActive returns whether or not the trigger should be active
+ */
+ public Trigger(BooleanSupplier isActive) {
+ m_isActive = isActive;
+ }
+
+ /**
+ * Creates a new trigger that is always inactive. Useful only as a no-arg constructor for
+ * subclasses that will be overriding {@link Trigger#get()} anyway.
+ */
+ public Trigger() {
+ m_isActive = () -> false;
+ }
+
+ /**
+ * Returns whether or not the trigger is active.
+ *
+ * <p>This method will be called repeatedly a command is linked to the Trigger.
+ *
+ * @return whether or not the trigger condition is active.
+ */
+ public boolean get() {
+ return m_isActive.getAsBoolean();
+ }
+
+ /**
+ * Starts the given command whenever the trigger just becomes active.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whenActive(final Command command, boolean interruptible) {
+ requireNonNullParam(command, "command", "whenActive");
+
+ CommandScheduler.getInstance().addButton(new Runnable() {
+ private boolean m_pressedLast = get();
+
+ @Override
+ public void run() {
+ boolean pressed = get();
+
+ if (!m_pressedLast && pressed) {
+ command.schedule(interruptible);
+ }
+
+ m_pressedLast = pressed;
+ }
+ });
+
+ return this;
+ }
+
+ /**
+ * Starts the given command whenever the trigger just becomes active. The command is set to be
+ * interruptible.
+ *
+ * @param command the command to start
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whenActive(final Command command) {
+ return whenActive(command, true);
+ }
+
+ /**
+ * Runs the given runnable whenever the trigger just becomes active.
+ *
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whenActive(final Runnable toRun, Subsystem... requirements) {
+ return whenActive(new InstantCommand(toRun, requirements));
+ }
+
+ /**
+ * Constantly starts the given command while the button is held.
+ *
+ * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
+ * will be canceled when the trigger becomes inactive.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
+ requireNonNullParam(command, "command", "whileActiveContinuous");
+
+ CommandScheduler.getInstance().addButton(new Runnable() {
+ private boolean m_pressedLast = get();
+
+ @Override
+ public void run() {
+ boolean pressed = get();
+
+ if (pressed) {
+ command.schedule(interruptible);
+ } else if (m_pressedLast) {
+ command.cancel();
+ }
+
+ m_pressedLast = pressed;
+ }
+ });
+ return this;
+ }
+
+ /**
+ * Constantly starts the given command while the button is held.
+ *
+ * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
+ * will be canceled when the trigger becomes inactive. The command is set to be interruptible.
+ *
+ * @param command the command to start
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whileActiveContinuous(final Command command) {
+ return whileActiveContinuous(command, true);
+ }
+
+ /**
+ * Constantly runs the given runnable while the button is held.
+ *
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whileActiveContinuous(final Runnable toRun, Subsystem... requirements) {
+ return whileActiveContinuous(new InstantCommand(toRun, requirements));
+ }
+
+ /**
+ * Starts the given command when the trigger initially becomes active, and ends it when it becomes
+ * inactive, but does not re-start it in-between.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whileActiveOnce(final Command command, boolean interruptible) {
+ requireNonNullParam(command, "command", "whileActiveOnce");
+
+ CommandScheduler.getInstance().addButton(new Runnable() {
+ private boolean m_pressedLast = get();
+
+ @Override
+ public void run() {
+ boolean pressed = get();
+
+ if (!m_pressedLast && pressed) {
+ command.schedule(interruptible);
+ } else if (m_pressedLast && !pressed) {
+ command.cancel();
+ }
+
+ m_pressedLast = pressed;
+ }
+ });
+ return this;
+ }
+
+ /**
+ * Starts the given command when the trigger initially becomes active, and ends it when it becomes
+ * inactive, but does not re-start it in-between. The command is set to be interruptible.
+ *
+ * @param command the command to start
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whileActiveOnce(final Command command) {
+ return whileActiveOnce(command, true);
+ }
+
+ /**
+ * Starts the command when the trigger becomes inactive.
+ *
+ * @param command the command to start
+ * @param interruptible whether the command is interruptible
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whenInactive(final Command command, boolean interruptible) {
+ requireNonNullParam(command, "command", "whenInactive");
+
+ CommandScheduler.getInstance().addButton(new Runnable() {
+ private boolean m_pressedLast = get();
+
+ @Override
+ public void run() {
+ boolean pressed = get();
+
+ if (m_pressedLast && !pressed) {
+ command.schedule(interruptible);
+ }
+
+ m_pressedLast = pressed;
+ }
+ });
+ return this;
+ }
+
+ /**
+ * Starts the command when the trigger becomes inactive. The command is set to be interruptible.
+ *
+ * @param command the command to start
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whenInactive(final Command command) {
+ return whenInactive(command, true);
+ }
+
+ /**
+ * Runs the given runnable when the trigger becomes inactive.
+ *
+ * @param toRun the runnable to run
+ * @param requirements the required subsystems
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger whenInactive(final Runnable toRun, Subsystem... requirements) {
+ return whenInactive(new InstantCommand(toRun, requirements));
+ }
+
+ /**
+ * Toggles a command when the trigger becomes active.
+ *
+ * @param command the command to toggle
+ * @param interruptible whether the command is interruptible
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger toggleWhenActive(final Command command, boolean interruptible) {
+ requireNonNullParam(command, "command", "toggleWhenActive");
+
+ CommandScheduler.getInstance().addButton(new Runnable() {
+ private boolean m_pressedLast = get();
+
+ @Override
+ public void run() {
+ boolean pressed = get();
+
+ if (!m_pressedLast && pressed) {
+ if (command.isScheduled()) {
+ command.cancel();
+ } else {
+ command.schedule(interruptible);
+ }
+ }
+
+ m_pressedLast = pressed;
+ }
+ });
+ return this;
+ }
+
+ /**
+ * Toggles a command when the trigger becomes active. The command is set to be interruptible.
+ *
+ * @param command the command to toggle
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger toggleWhenActive(final Command command) {
+ return toggleWhenActive(command, true);
+ }
+
+ /**
+ * Cancels a command when the trigger becomes active.
+ *
+ * @param command the command to cancel
+ * @return this trigger, so calls can be chained
+ */
+ public Trigger cancelWhenActive(final Command command) {
+ requireNonNullParam(command, "command", "cancelWhenActive");
+
+ CommandScheduler.getInstance().addButton(new Runnable() {
+ private boolean m_pressedLast = get();
+
+ @Override
+ public void run() {
+ boolean pressed = get();
+
+ if (!m_pressedLast && pressed) {
+ command.cancel();
+ }
+
+ m_pressedLast = pressed;
+ }
+ });
+ return this;
+ }
+
+ /**
+ * Composes this trigger with another trigger, returning a new trigger that is active when both
+ * triggers are active.
+ *
+ * @param trigger the trigger to compose with
+ * @return the trigger that is active when both triggers are active
+ */
+ public Trigger and(Trigger trigger) {
+ return new Trigger(() -> get() && trigger.get());
+ }
+
+ /**
+ * Composes this trigger with another trigger, returning a new trigger that is active when either
+ * trigger is active.
+ *
+ * @param trigger the trigger to compose with
+ * @return the trigger that is active when either trigger is active
+ */
+ public Trigger or(Trigger trigger) {
+ return new Trigger(() -> get() || trigger.get());
+ }
+
+ /**
+ * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the
+ * negation of this trigger.
+ *
+ * @return the negated trigger
+ */
+ public Trigger negate() {
+ return new Trigger(() -> !get());
+ }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
new file mode 100644
index 0000000..83c9721
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Command.h"
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
+
+Command::Command(const Command& rhs) : ErrorBase(rhs) {}
+
+Command& Command::operator=(const Command& rhs) {
+ ErrorBase::operator=(rhs);
+ m_isGrouped = false;
+ return *this;
+}
+
+void Command::Initialize() {}
+void Command::Execute() {}
+void Command::End(bool interrupted) {}
+
+ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::make_unique<WaitCommand>(duration));
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ return ParallelRaceGroup(std::move(temp));
+}
+
+ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ return ParallelRaceGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::BeforeStarting(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(
+ std::make_unique<InstantCommand>(std::move(toRun), requirements));
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ return SequentialCommandGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::AndThen(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ temp.emplace_back(
+ std::make_unique<InstantCommand>(std::move(toRun), requirements));
+ return SequentialCommandGroup(std::move(temp));
+}
+
+PerpetualCommand Command::Perpetually() && {
+ return PerpetualCommand(std::move(*this).TransferOwnership());
+}
+
+ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
+
+void Command::Schedule(bool interruptible) {
+ CommandScheduler::GetInstance().Schedule(interruptible, this);
+}
+
+void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
+
+bool Command::IsScheduled() const {
+ return CommandScheduler::GetInstance().IsScheduled(this);
+}
+
+bool Command::HasRequirement(Subsystem* requirement) const {
+ bool hasRequirement = false;
+ for (auto&& subsystem : GetRequirements()) {
+ hasRequirement |= requirement == subsystem;
+ }
+ return hasRequirement;
+}
+
+std::string Command::GetName() const { return GetTypeName(*this); }
+
+bool Command::IsGrouped() const { return m_isGrouped; }
+
+void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
+
+namespace frc2 {
+bool RequirementsDisjoint(Command* first, Command* second) {
+ bool disjoint = true;
+ auto&& requirements = second->GetRequirements();
+ for (auto&& requirement : first->GetRequirements()) {
+ disjoint &= requirements.find(requirement) == requirements.end();
+ }
+ return disjoint;
+}
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
new file mode 100644
index 0000000..323ed67
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+
+using namespace frc2;
+
+CommandBase::CommandBase() {
+ frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+}
+
+void CommandBase::AddRequirements(
+ std::initializer_list<Subsystem*> requirements) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
+ return m_requirements;
+}
+
+void CommandBase::SetName(const wpi::Twine& name) {
+ frc::SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string CommandBase::GetName() const {
+ return frc::SendableRegistry::GetInstance().GetName(this);
+}
+
+std::string CommandBase::GetSubsystem() const {
+ return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
+ frc::SendableRegistry::GetInstance().SetSubsystem(this, subsystem);
+}
+
+void CommandBase::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Command");
+ builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
+ builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
+ [this](bool value) {
+ bool isScheduled = IsScheduled();
+ if (value && !isScheduled) {
+ Schedule();
+ } else if (!value && isScheduled) {
+ Cancel();
+ }
+ });
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
new file mode 100644
index 0000000..8869d4c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandGroupBase.h"
+
+#include <frc/WPIErrors.h>
+
+using namespace frc2;
+
+bool CommandGroupBase::RequireUngrouped(Command& command) {
+ if (command.IsGrouped()) {
+ wpi_setGlobalWPIErrorWithContext(
+ CommandIllegalUse,
+ "Commands cannot be added to more than one CommandGroup");
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool CommandGroupBase::RequireUngrouped(
+ wpi::ArrayRef<std::unique_ptr<Command>> commands) {
+ bool allUngrouped = true;
+ for (auto&& command : commands) {
+ allUngrouped &= !command.get()->IsGrouped();
+ }
+ if (!allUngrouped) {
+ wpi_setGlobalWPIErrorWithContext(
+ CommandIllegalUse,
+ "Commands cannot be added to more than one CommandGroup");
+ }
+ return allUngrouped;
+}
+
+bool CommandGroupBase::RequireUngrouped(
+ std::initializer_list<Command*> commands) {
+ bool allUngrouped = true;
+ for (auto&& command : commands) {
+ allUngrouped &= !command->IsGrouped();
+ }
+ if (!allUngrouped) {
+ wpi_setGlobalWPIErrorWithContext(
+ CommandIllegalUse,
+ "Commands cannot be added to more than one CommandGroup");
+ }
+ return allUngrouped;
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
new file mode 100644
index 0000000..84d0820
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -0,0 +1,399 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandScheduler.h"
+
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <hal/HALBase.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandState.h"
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+
+class CommandScheduler::Impl {
+ public:
+ // A map from commands to their scheduling state. Also used as a set of the
+ // currently-running commands.
+ wpi::DenseMap<Command*, CommandState> scheduledCommands;
+
+ // A map from required subsystems to their requiring commands. Also used as a
+ // set of the currently-required subsystems.
+ wpi::DenseMap<Subsystem*, Command*> requirements;
+
+ // A map from subsystems registered with the scheduler to their default
+ // commands. Also used as a list of currently-registered subsystems.
+ wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> subsystems;
+
+ // The set of currently-registered buttons that will be polled every
+ // iteration.
+ wpi::SmallVector<wpi::unique_function<void()>, 4> buttons;
+
+ bool disabled{false};
+
+ // NetworkTable entries for use in Sendable impl
+ nt::NetworkTableEntry namesEntry;
+ nt::NetworkTableEntry idsEntry;
+ nt::NetworkTableEntry cancelEntry;
+
+ // Lists of user-supplied actions to be executed on scheduling events for
+ // every command.
+ wpi::SmallVector<Action, 4> initActions;
+ wpi::SmallVector<Action, 4> executeActions;
+ wpi::SmallVector<Action, 4> interruptActions;
+ wpi::SmallVector<Action, 4> finishActions;
+
+ // Flag and queues for avoiding concurrent modification if commands are
+ // scheduled/canceled during run
+
+ bool inRunLoop = false;
+ wpi::DenseMap<Command*, bool> toSchedule;
+ wpi::SmallVector<Command*, 4> toCancel;
+};
+
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+ return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler() : m_impl(new Impl) {
+ frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+CommandScheduler::~CommandScheduler() {}
+
+CommandScheduler& CommandScheduler::GetInstance() {
+ static CommandScheduler scheduler;
+ return scheduler;
+}
+
+void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
+ m_impl->buttons.emplace_back(std::move(button));
+}
+
+void CommandScheduler::ClearButtons() { m_impl->buttons.clear(); }
+
+void CommandScheduler::Schedule(bool interruptible, Command* command) {
+ if (m_impl->inRunLoop) {
+ m_impl->toSchedule.try_emplace(command, interruptible);
+ return;
+ }
+
+ if (command->IsGrouped()) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "A command that is part of a command group "
+ "cannot be independently scheduled");
+ return;
+ }
+ if (m_impl->disabled ||
+ (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
+ ContainsKey(m_impl->scheduledCommands, command)) {
+ return;
+ }
+
+ const auto& requirements = command->GetRequirements();
+
+ wpi::SmallVector<Command*, 8> intersection;
+
+ bool isDisjoint = true;
+ bool allInterruptible = true;
+ for (auto&& i1 : m_impl->requirements) {
+ if (requirements.find(i1.first) != requirements.end()) {
+ isDisjoint = false;
+ allInterruptible &=
+ m_impl->scheduledCommands[i1.second].IsInterruptible();
+ intersection.emplace_back(i1.second);
+ }
+ }
+
+ if (isDisjoint || allInterruptible) {
+ if (allInterruptible) {
+ for (auto&& cmdToCancel : intersection) {
+ Cancel(cmdToCancel);
+ }
+ }
+ command->Initialize();
+ m_impl->scheduledCommands[command] = CommandState{interruptible};
+ for (auto&& action : m_impl->initActions) {
+ action(*command);
+ }
+ for (auto&& requirement : requirements) {
+ m_impl->requirements[requirement] = command;
+ }
+ }
+}
+
+void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
+
+void CommandScheduler::Schedule(bool interruptible,
+ wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(interruptible, command);
+ }
+}
+
+void CommandScheduler::Schedule(bool interruptible,
+ std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(interruptible, command);
+ }
+}
+
+void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(true, command);
+ }
+}
+
+void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(true, command);
+ }
+}
+
+void CommandScheduler::Run() {
+ if (m_impl->disabled) {
+ return;
+ }
+
+ // Run the periodic method of all registered subsystems.
+ for (auto&& subsystem : m_impl->subsystems) {
+ subsystem.getFirst()->Periodic();
+ }
+
+ // Poll buttons for new commands to add.
+ for (auto&& button : m_impl->buttons) {
+ button();
+ }
+
+ m_impl->inRunLoop = true;
+ // Run scheduled commands, remove finished commands.
+ for (auto iterator = m_impl->scheduledCommands.begin();
+ iterator != m_impl->scheduledCommands.end(); iterator++) {
+ Command* command = iterator->getFirst();
+
+ if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
+ Cancel(command);
+ continue;
+ }
+
+ command->Execute();
+ for (auto&& action : m_impl->executeActions) {
+ action(*command);
+ }
+
+ if (command->IsFinished()) {
+ command->End(false);
+ for (auto&& action : m_impl->finishActions) {
+ action(*command);
+ }
+
+ for (auto&& requirement : command->GetRequirements()) {
+ m_impl->requirements.erase(requirement);
+ }
+
+ m_impl->scheduledCommands.erase(iterator);
+ }
+ }
+ m_impl->inRunLoop = false;
+
+ for (auto&& commandInterruptible : m_impl->toSchedule) {
+ Schedule(commandInterruptible.second, commandInterruptible.first);
+ }
+
+ for (auto&& command : m_impl->toCancel) {
+ Cancel(command);
+ }
+
+ m_impl->toSchedule.clear();
+ m_impl->toCancel.clear();
+
+ // Add default commands for un-required registered subsystems.
+ for (auto&& subsystem : m_impl->subsystems) {
+ auto s = m_impl->requirements.find(subsystem.getFirst());
+ if (s == m_impl->requirements.end() && subsystem.getSecond()) {
+ Schedule({subsystem.getSecond().get()});
+ }
+ }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+ m_impl->subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+ auto s = m_impl->subsystems.find(subsystem);
+ if (s != m_impl->subsystems.end()) {
+ m_impl->subsystems.erase(s);
+ }
+}
+
+void CommandScheduler::RegisterSubsystem(
+ std::initializer_list<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ RegisterSubsystem(subsystem);
+ }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+ std::initializer_list<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ UnregisterSubsystem(subsystem);
+ }
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+ auto&& find = m_impl->subsystems.find(subsystem);
+ if (find != m_impl->subsystems.end()) {
+ return find->second.get();
+ } else {
+ return nullptr;
+ }
+}
+
+void CommandScheduler::Cancel(Command* command) {
+ if (m_impl->inRunLoop) {
+ m_impl->toCancel.emplace_back(command);
+ return;
+ }
+
+ auto find = m_impl->scheduledCommands.find(command);
+ if (find == m_impl->scheduledCommands.end()) return;
+ command->End(true);
+ for (auto&& action : m_impl->interruptActions) {
+ action(*command);
+ }
+ m_impl->scheduledCommands.erase(find);
+ for (auto&& requirement : m_impl->requirements) {
+ if (requirement.second == command) {
+ m_impl->requirements.erase(requirement.first);
+ }
+ }
+}
+
+void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Cancel(command);
+ }
+}
+
+void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Cancel(command);
+ }
+}
+
+void CommandScheduler::CancelAll() {
+ for (auto&& command : m_impl->scheduledCommands) {
+ Cancel(command.first);
+ }
+}
+
+double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+ auto find = m_impl->scheduledCommands.find(command);
+ if (find != m_impl->scheduledCommands.end()) {
+ return find->second.TimeSinceInitialized();
+ } else {
+ return -1;
+ }
+}
+bool CommandScheduler::IsScheduled(
+ wpi::ArrayRef<const Command*> commands) const {
+ for (auto command : commands) {
+ if (!IsScheduled(command)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool CommandScheduler::IsScheduled(
+ std::initializer_list<const Command*> commands) const {
+ for (auto command : commands) {
+ if (!IsScheduled(command)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool CommandScheduler::IsScheduled(const Command* command) const {
+ return m_impl->scheduledCommands.find(command) !=
+ m_impl->scheduledCommands.end();
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+ auto find = m_impl->requirements.find(subsystem);
+ if (find != m_impl->requirements.end()) {
+ return find->second;
+ } else {
+ return nullptr;
+ }
+}
+
+void CommandScheduler::Disable() { m_impl->disabled = true; }
+
+void CommandScheduler::Enable() { m_impl->disabled = false; }
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+ m_impl->initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+ m_impl->executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+ m_impl->interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+ m_impl->finishActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Scheduler");
+ m_impl->namesEntry = builder.GetEntry("Names");
+ m_impl->idsEntry = builder.GetEntry("Ids");
+ m_impl->cancelEntry = builder.GetEntry("Cancel");
+
+ builder.SetUpdateTable([this] {
+ double tmp[1];
+ tmp[0] = 0;
+ auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
+ for (auto cancel : toCancel) {
+ uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
+ Command* command = reinterpret_cast<Command*>(ptrTmp);
+ if (m_impl->scheduledCommands.find(command) !=
+ m_impl->scheduledCommands.end()) {
+ Cancel(command);
+ }
+ m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+ }
+
+ wpi::SmallVector<std::string, 8> names;
+ wpi::SmallVector<double, 8> ids;
+ for (auto&& command : m_impl->scheduledCommands) {
+ names.emplace_back(command.first->GetName());
+ uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
+ ids.emplace_back(static_cast<double>(ptrTmp));
+ }
+ m_impl->namesEntry.SetStringArray(names);
+ m_impl->idsEntry.SetDoubleArray(ids);
+ });
+}
+
+void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
+ std::unique_ptr<Command> command) {
+ m_impl->subsystems[subsystem] = std::move(command);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
new file mode 100644
index 0000000..b41fa70
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandState.h"
+
+#include <frc/Timer.h>
+
+using namespace frc2;
+CommandState::CommandState(bool interruptible)
+ : m_interruptible{interruptible} {
+ StartTiming();
+ StartRunning();
+}
+
+void CommandState::StartTiming() {
+ m_startTime = frc::Timer::GetFPGATimestamp();
+}
+void CommandState::StartRunning() { m_startTime = -1; }
+double CommandState::TimeSinceInitialized() const {
+ return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
new file mode 100644
index 0000000..2344513
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ConditionalCommand.h"
+
+using namespace frc2;
+
+ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+ std::unique_ptr<Command>&& onFalse,
+ std::function<bool()> condition)
+ : m_condition{std::move(condition)} {
+ if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) {
+ return;
+ }
+
+ m_onTrue = std::move(onTrue);
+ m_onFalse = std::move(onFalse);
+
+ m_onTrue->SetGrouped(true);
+ m_onFalse->SetGrouped(true);
+
+ m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
+ m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
+
+ AddRequirements(m_onTrue->GetRequirements());
+ AddRequirements(m_onFalse->GetRequirements());
+}
+
+void ConditionalCommand::Initialize() {
+ if (m_condition()) {
+ m_selectedCommand = m_onTrue.get();
+ } else {
+ m_selectedCommand = m_onFalse.get();
+ }
+ m_selectedCommand->Initialize();
+}
+
+void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
+
+void ConditionalCommand::End(bool interrupted) {
+ m_selectedCommand->End(interrupted);
+}
+
+bool ConditionalCommand::IsFinished() {
+ return m_selectedCommand->IsFinished();
+}
+
+bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
new file mode 100644
index 0000000..ef7c40a
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+
+FunctionalCommand::FunctionalCommand(
+ std::function<void()> onInit, std::function<void()> onExecute,
+ std::function<void(bool)> onEnd, std::function<bool()> isFinished,
+ std::initializer_list<Subsystem*> requirements)
+ : m_onInit{std::move(onInit)},
+ m_onExecute{std::move(onExecute)},
+ m_onEnd{std::move(onEnd)},
+ m_isFinished{std::move(isFinished)} {
+ AddRequirements(requirements);
+}
+
+void FunctionalCommand::Initialize() { m_onInit(); }
+
+void FunctionalCommand::Execute() { m_onExecute(); }
+
+void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
+
+bool FunctionalCommand::IsFinished() { return m_isFinished(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
new file mode 100644
index 0000000..b199074
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+
+InstantCommand::InstantCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements)
+ : m_toRun{std::move(toRun)} {
+ AddRequirements(requirements);
+}
+
+InstantCommand::InstantCommand() : m_toRun{[] {}} {}
+
+void InstantCommand::Initialize() { m_toRun(); }
+
+bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
new file mode 100644
index 0000000..15fb254
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/MecanumControllerCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+MecanumControllerCommand::MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+ frc2::PIDController frontLeftController,
+ frc2::PIDController rearLeftController,
+ frc2::PIDController frontRightController,
+ frc2::PIDController rearRightController,
+ std::function<void(units::volt_t, units::volt_t, units::volt_t,
+ units::volt_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_feedforward(feedforward),
+ m_kinematics(kinematics),
+ m_xController(std::make_unique<frc2::PIDController>(xController)),
+ m_yController(std::make_unique<frc2::PIDController>(yController)),
+ m_thetaController(
+ std::make_unique<frc::ProfiledPIDController<units::radians>>(
+ thetaController)),
+ m_maxWheelVelocity(maxWheelVelocity),
+ m_frontLeftController(
+ std::make_unique<frc2::PIDController>(frontLeftController)),
+ m_rearLeftController(
+ std::make_unique<frc2::PIDController>(rearLeftController)),
+ m_frontRightController(
+ std::make_unique<frc2::PIDController>(frontRightController)),
+ m_rearRightController(
+ std::make_unique<frc2::PIDController>(rearRightController)),
+ m_currentWheelSpeeds(currentWheelSpeeds),
+ m_outputVolts(output),
+ m_usePID(true) {
+ AddRequirements(requirements);
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+ units::meters_per_second_t, units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_kinematics(kinematics),
+ m_xController(std::make_unique<frc2::PIDController>(xController)),
+ m_yController(std::make_unique<frc2::PIDController>(yController)),
+ m_thetaController(
+ std::make_unique<frc::ProfiledPIDController<units::radians>>(
+ thetaController)),
+ m_maxWheelVelocity(maxWheelVelocity),
+ m_outputVel(output),
+ m_usePID(false) {
+ AddRequirements(requirements);
+}
+
+void MecanumControllerCommand::Initialize() {
+ m_prevTime = 0_s;
+ auto initialState = m_trajectory.Sample(0_s);
+
+ auto initialXVelocity =
+ initialState.velocity * initialState.pose.Rotation().Cos();
+ auto initialYVelocity =
+ initialState.velocity * initialState.pose.Rotation().Sin();
+
+ m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+ frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s});
+
+ m_timer.Reset();
+ m_timer.Start();
+ if (m_usePID) {
+ m_frontLeftController->Reset();
+ m_rearLeftController->Reset();
+ m_frontRightController->Reset();
+ m_rearRightController->Reset();
+ }
+}
+
+void MecanumControllerCommand::Execute() {
+ auto curTime = second_t(m_timer.Get());
+ auto dt = curTime - m_prevTime;
+
+ auto m_desiredState = m_trajectory.Sample(curTime);
+ auto m_desiredPose = m_desiredState.pose;
+
+ auto m_poseError = m_desiredPose.RelativeTo(m_pose());
+
+ auto targetXVel = meters_per_second_t(
+ m_xController->Calculate((m_pose().Translation().X().to<double>()),
+ (m_desiredPose.Translation().X().to<double>())));
+ auto targetYVel = meters_per_second_t(
+ m_yController->Calculate((m_pose().Translation().Y().to<double>()),
+ (m_desiredPose.Translation().Y().to<double>())));
+
+ // Profiled PID Controller only takes meters as setpoint and measurement
+ // The robot will go to the desired rotation of the final pose in the
+ // trajectory, not following the poses at individual states.
+ auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate(
+ m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
+
+ auto vRef = m_desiredState.velocity;
+
+ targetXVel += vRef * m_poseError.Rotation().Cos();
+ targetYVel += vRef * m_poseError.Rotation().Sin();
+
+ auto targetChassisSpeeds =
+ frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
+
+ auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
+
+ targetWheelSpeeds.Normalize(m_maxWheelVelocity);
+
+ auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
+ auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
+ auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
+ auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
+
+ if (m_usePID) {
+ auto frontLeftFeedforward = m_feedforward.Calculate(
+ frontLeftSpeedSetpoint,
+ (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt);
+
+ auto rearLeftFeedforward = m_feedforward.Calculate(
+ rearLeftSpeedSetpoint,
+ (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt);
+
+ auto frontRightFeedforward = m_feedforward.Calculate(
+ frontRightSpeedSetpoint,
+ (frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt);
+
+ auto rearRightFeedforward = m_feedforward.Calculate(
+ rearRightSpeedSetpoint,
+ (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
+
+ auto frontLeftOutput = volt_t(m_frontLeftController->Calculate(
+ m_currentWheelSpeeds().frontLeft.to<double>(),
+ frontLeftSpeedSetpoint.to<double>())) +
+ frontLeftFeedforward;
+ auto rearLeftOutput = volt_t(m_rearLeftController->Calculate(
+ m_currentWheelSpeeds().rearLeft.to<double>(),
+ rearLeftSpeedSetpoint.to<double>())) +
+ rearLeftFeedforward;
+ auto frontRightOutput = volt_t(m_frontRightController->Calculate(
+ m_currentWheelSpeeds().frontRight.to<double>(),
+ frontRightSpeedSetpoint.to<double>())) +
+ frontRightFeedforward;
+ auto rearRightOutput = volt_t(m_rearRightController->Calculate(
+ m_currentWheelSpeeds().rearRight.to<double>(),
+ rearRightSpeedSetpoint.to<double>())) +
+ rearRightFeedforward;
+
+ m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput,
+ rearRightOutput);
+ } else {
+ m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint,
+ frontRightSpeedSetpoint, rearRightSpeedSetpoint);
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+ }
+}
+
+void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool MecanumControllerCommand::IsFinished() {
+ return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
new file mode 100644
index 0000000..c4eac81
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+
+NotifierCommand::NotifierCommand(std::function<void()> toRun,
+ units::second_t period,
+ std::initializer_list<Subsystem*> requirements)
+ : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+ AddRequirements(requirements);
+}
+
+NotifierCommand::NotifierCommand(NotifierCommand&& other)
+ : CommandHelper(std::move(other)),
+ m_toRun(other.m_toRun),
+ m_notifier(other.m_toRun),
+ m_period(other.m_period) {}
+
+NotifierCommand::NotifierCommand(const NotifierCommand& other)
+ : CommandHelper(other),
+ m_toRun(other.m_toRun),
+ m_notifier(frc::Notifier(other.m_toRun)),
+ m_period(other.m_period) {}
+
+void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
+
+void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
new file mode 100644
index 0000000..4a956f6
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDCommand.h"
+
+using namespace frc2;
+
+PIDCommand::PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ std::function<double()> setpointSource,
+ std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : m_controller{controller},
+ m_measurement{std::move(measurementSource)},
+ m_setpoint{std::move(setpointSource)},
+ m_useOutput{std::move(useOutput)} {
+ AddRequirements(requirements);
+}
+
+PIDCommand::PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ double setpoint, std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
+ useOutput, requirements) {}
+
+void PIDCommand::Initialize() { m_controller.Reset(); }
+
+void PIDCommand::Execute() {
+ m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint()));
+}
+
+void PIDCommand::End(bool interrupted) { m_useOutput(0); }
+
+PIDController& PIDCommand::GetController() { return m_controller; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
new file mode 100644
index 0000000..ce8198a
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDSubsystem.h"
+
+using namespace frc2;
+
+PIDSubsystem::PIDSubsystem(PIDController controller)
+ : m_controller{controller} {}
+
+void PIDSubsystem::Periodic() {
+ if (m_enabled) {
+ UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint), m_setpoint);
+ }
+}
+
+void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+
+void PIDSubsystem::Enable() {
+ m_controller.Reset();
+ m_enabled = true;
+}
+
+void PIDSubsystem::Disable() {
+ UseOutput(0, 0);
+ m_enabled = false;
+}
+
+bool PIDSubsystem::IsEnabled() { return m_enabled; }
+
+PIDController& PIDSubsystem::GetController() { return m_controller; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
new file mode 100644
index 0000000..557984e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelCommandGroup.h"
+
+using namespace frc2;
+
+ParallelCommandGroup::ParallelCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ AddCommands(std::move(commands));
+}
+
+void ParallelCommandGroup::Initialize() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning.first->Initialize();
+ commandRunning.second = true;
+ }
+ isRunning = true;
+}
+
+void ParallelCommandGroup::Execute() {
+ for (auto& commandRunning : m_commands) {
+ if (!commandRunning.second) continue;
+ commandRunning.first->Execute();
+ if (commandRunning.first->IsFinished()) {
+ commandRunning.first->End(false);
+ commandRunning.second = false;
+ }
+ }
+}
+
+void ParallelCommandGroup::End(bool interrupted) {
+ if (interrupted) {
+ for (auto& commandRunning : m_commands) {
+ if (commandRunning.second) {
+ commandRunning.first->End(true);
+ }
+ }
+ }
+ isRunning = false;
+}
+
+bool ParallelCommandGroup::IsFinished() {
+ for (auto& command : m_commands) {
+ if (command.second) return false;
+ }
+ return true;
+}
+
+bool ParallelCommandGroup::RunsWhenDisabled() const {
+ return m_runWhenDisabled;
+}
+
+void ParallelCommandGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ for (auto&& command : commands) {
+ if (!RequireUngrouped(*command)) return;
+ }
+
+ if (isRunning) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ if (RequirementsDisjoint(this, command.get())) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands.emplace_back(std::move(command), false);
+ } else {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Multiple commands in a parallel group cannot "
+ "require the same subsystems");
+ return;
+ }
+ }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
new file mode 100644
index 0000000..935cf8b
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelDeadlineGroup.h"
+
+using namespace frc2;
+
+ParallelDeadlineGroup::ParallelDeadlineGroup(
+ std::unique_ptr<Command>&& deadline,
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ SetDeadline(std::move(deadline));
+ AddCommands(std::move(commands));
+}
+
+void ParallelDeadlineGroup::Initialize() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning.first->Initialize();
+ commandRunning.second = true;
+ }
+ m_finished = false;
+}
+
+void ParallelDeadlineGroup::Execute() {
+ for (auto& commandRunning : m_commands) {
+ if (!commandRunning.second) continue;
+ commandRunning.first->Execute();
+ if (commandRunning.first->IsFinished()) {
+ commandRunning.first->End(false);
+ commandRunning.second = false;
+ if (commandRunning.first.get() == m_deadline) {
+ m_finished = true;
+ }
+ }
+ }
+}
+
+void ParallelDeadlineGroup::End(bool interrupted) {
+ for (auto& commandRunning : m_commands) {
+ if (commandRunning.second) {
+ commandRunning.first->End(true);
+ }
+ }
+}
+
+bool ParallelDeadlineGroup::IsFinished() { return m_finished; }
+
+bool ParallelDeadlineGroup::RunsWhenDisabled() const {
+ return m_runWhenDisabled;
+}
+
+void ParallelDeadlineGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ if (!RequireUngrouped(commands)) {
+ return;
+ }
+
+ if (!m_finished) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ if (RequirementsDisjoint(this, command.get())) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands.emplace_back(std::move(command), false);
+ } else {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Multiple commands in a parallel group cannot "
+ "require the same subsystems");
+ return;
+ }
+ }
+}
+
+void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
+ m_deadline = deadline.get();
+ m_deadline->SetGrouped(true);
+ m_commands.emplace_back(std::move(deadline), false);
+ AddRequirements(m_deadline->GetRequirements());
+ m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
new file mode 100644
index 0000000..466cc79
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelRaceGroup.h"
+
+using namespace frc2;
+
+ParallelRaceGroup::ParallelRaceGroup(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ AddCommands(std::move(commands));
+}
+
+void ParallelRaceGroup::Initialize() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning->Initialize();
+ }
+ isRunning = true;
+}
+
+void ParallelRaceGroup::Execute() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning->Execute();
+ if (commandRunning->IsFinished()) {
+ m_finished = true;
+ }
+ }
+}
+
+void ParallelRaceGroup::End(bool interrupted) {
+ for (auto& commandRunning : m_commands) {
+ commandRunning->End(!commandRunning->IsFinished());
+ }
+ isRunning = false;
+}
+
+bool ParallelRaceGroup::IsFinished() { return m_finished; }
+
+bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
+
+void ParallelRaceGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ if (!RequireUngrouped(commands)) {
+ return;
+ }
+
+ if (isRunning) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ if (RequirementsDisjoint(this, command.get())) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands.emplace_back(std::move(command));
+ } else {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Multiple commands in a parallel group cannot "
+ "require the same subsystems");
+ return;
+ }
+ }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
new file mode 100644
index 0000000..f29850b
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+
+PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
+ if (!CommandGroupBase::RequireUngrouped(command)) {
+ return;
+ }
+ m_command = std::move(command);
+ m_command->SetGrouped(true);
+ AddRequirements(m_command->GetRequirements());
+}
+
+void PerpetualCommand::Initialize() { m_command->Initialize(); }
+
+void PerpetualCommand::Execute() { m_command->Execute(); }
+
+void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
new file mode 100644
index 0000000..e077760
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PrintCommand.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc2;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+ : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+}
+
+bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
new file mode 100644
index 0000000..6f96315
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProxyScheduleCommand.h"
+
+using namespace frc2;
+
+ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+ SetInsert(m_toSchedule, toSchedule);
+}
+
+void ProxyScheduleCommand::Initialize() {
+ for (auto* command : m_toSchedule) {
+ command->Schedule();
+ }
+}
+
+void ProxyScheduleCommand::End(bool interrupted) {
+ if (interrupted) {
+ for (auto* command : m_toSchedule) {
+ command->Cancel();
+ }
+ }
+}
+
+void ProxyScheduleCommand::Execute() {
+ m_finished = true;
+ for (auto* command : m_toSchedule) {
+ m_finished &= !command->IsScheduled();
+ }
+}
+
+bool ProxyScheduleCommand::IsFinished() { return m_finished; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
new file mode 100644
index 0000000..261896f
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RamseteCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+RamseteCommand::RamseteCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+ frc2::PIDController leftController, frc2::PIDController rightController,
+ std::function<void(volt_t, volt_t)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_controller(controller),
+ m_feedforward(feedforward),
+ m_kinematics(kinematics),
+ m_speeds(wheelSpeeds),
+ m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+ m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+ m_outputVolts(output),
+ m_usePID(true) {
+ AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_controller(controller),
+ m_kinematics(kinematics),
+ m_outputVel(output),
+ m_usePID(false) {
+ AddRequirements(requirements);
+}
+
+void RamseteCommand::Initialize() {
+ m_prevTime = 0_s;
+ auto initialState = m_trajectory.Sample(0_s);
+ m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+ frc::ChassisSpeeds{initialState.velocity, 0_mps,
+ initialState.velocity * initialState.curvature});
+ m_timer.Reset();
+ m_timer.Start();
+ if (m_usePID) {
+ m_leftController->Reset();
+ m_rightController->Reset();
+ }
+}
+
+void RamseteCommand::Execute() {
+ auto curTime = m_timer.Get();
+ auto dt = curTime - m_prevTime;
+
+ auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
+ m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
+
+ if (m_usePID) {
+ auto leftFeedforward = m_feedforward.Calculate(
+ targetWheelSpeeds.left,
+ (targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
+
+ auto rightFeedforward = m_feedforward.Calculate(
+ targetWheelSpeeds.right,
+ (targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
+
+ auto leftOutput = volt_t(m_leftController->Calculate(
+ m_speeds().left.to<double>(),
+ targetWheelSpeeds.left.to<double>())) +
+ leftFeedforward;
+
+ auto rightOutput = volt_t(m_rightController->Calculate(
+ m_speeds().right.to<double>(),
+ targetWheelSpeeds.right.to<double>())) +
+ rightFeedforward;
+
+ m_outputVolts(leftOutput, rightOutput);
+ } else {
+ m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
+ }
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+}
+
+void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool RamseteCommand::IsFinished() {
+ return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
new file mode 100644
index 0000000..5c2c755
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+
+RunCommand::RunCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements)
+ : m_toRun{std::move(toRun)} {
+ AddRequirements(requirements);
+}
+
+void RunCommand::Execute() { m_toRun(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
new file mode 100644
index 0000000..ea1ea8d
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+
+ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+ SetInsert(m_toSchedule, toSchedule);
+}
+
+void ScheduleCommand::Initialize() {
+ for (auto command : m_toSchedule) {
+ command->Schedule();
+ }
+}
+
+bool ScheduleCommand::IsFinished() { return true; }
+
+bool ScheduleCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
new file mode 100644
index 0000000..1aa19e4
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+
+SequentialCommandGroup::SequentialCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ AddCommands(std::move(commands));
+}
+
+void SequentialCommandGroup::Initialize() {
+ m_currentCommandIndex = 0;
+
+ if (!m_commands.empty()) {
+ m_commands[0]->Initialize();
+ }
+}
+
+void SequentialCommandGroup::Execute() {
+ if (m_commands.empty()) return;
+
+ auto& currentCommand = m_commands[m_currentCommandIndex];
+
+ currentCommand->Execute();
+ if (currentCommand->IsFinished()) {
+ currentCommand->End(false);
+ m_currentCommandIndex++;
+ if (m_currentCommandIndex < m_commands.size()) {
+ m_commands[m_currentCommandIndex]->Initialize();
+ }
+ }
+}
+
+void SequentialCommandGroup::End(bool interrupted) {
+ if (interrupted && !m_commands.empty() &&
+ m_currentCommandIndex != invalid_index &&
+ m_currentCommandIndex < m_commands.size()) {
+ m_commands[m_currentCommandIndex]->End(interrupted);
+ }
+ m_currentCommandIndex = invalid_index;
+}
+
+bool SequentialCommandGroup::IsFinished() {
+ return m_currentCommandIndex == m_commands.size();
+}
+
+bool SequentialCommandGroup::RunsWhenDisabled() const {
+ return m_runWhenDisabled;
+}
+
+void SequentialCommandGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ if (!RequireUngrouped(commands)) {
+ return;
+ }
+
+ if (m_currentCommandIndex != invalid_index) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands.emplace_back(std::move(command));
+ }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
new file mode 100644
index 0000000..33407bf
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+ std::function<void()> onEnd,
+ std::initializer_list<Subsystem*> requirements)
+ : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
+ AddRequirements(requirements);
+}
+
+StartEndCommand::StartEndCommand(const StartEndCommand& other)
+ : CommandHelper(other) {
+ m_onInit = other.m_onInit;
+ m_onEnd = other.m_onEnd;
+}
+
+void StartEndCommand::Initialize() { m_onInit(); }
+
+void StartEndCommand::End(bool interrupted) { m_onEnd(); }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
new file mode 100644
index 0000000..cccb55b
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+Subsystem::~Subsystem() {
+ CommandScheduler::GetInstance().UnregisterSubsystem(this);
+}
+
+void Subsystem::Periodic() {}
+
+Command* Subsystem::GetDefaultCommand() const {
+ return CommandScheduler::GetInstance().GetDefaultCommand(this);
+}
+
+Command* Subsystem::GetCurrentCommand() const {
+ return CommandScheduler::GetInstance().Requiring(this);
+}
+
+void Subsystem::Register() {
+ return CommandScheduler::GetInstance().RegisterSubsystem(this);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
new file mode 100644
index 0000000..5e5ecbf
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SubsystemBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
+
+using namespace frc2;
+
+SubsystemBase::SubsystemBase() {
+ frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+ CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
+void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Subsystem");
+ builder.AddBooleanProperty(".hasDefault",
+ [this] { return GetDefaultCommand() != nullptr; },
+ nullptr);
+ builder.AddStringProperty(".default",
+ [this]() -> std::string {
+ auto command = GetDefaultCommand();
+ if (command == nullptr) return "none";
+ return command->GetName();
+ },
+ nullptr);
+ builder.AddBooleanProperty(".hasCommand",
+ [this] { return GetCurrentCommand() != nullptr; },
+ nullptr);
+ builder.AddStringProperty(".command",
+ [this]() -> std::string {
+ auto command = GetCurrentCommand();
+ if (command == nullptr) return "none";
+ return command->GetName();
+ },
+ nullptr);
+}
+
+std::string SubsystemBase::GetName() const {
+ return frc::SendableRegistry::GetInstance().GetName(this);
+}
+
+void SubsystemBase::SetName(const wpi::Twine& name) {
+ frc::SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string SubsystemBase::GetSubsystem() const {
+ return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
+ frc::SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
+ auto& registry = frc::SendableRegistry::GetInstance();
+ registry.AddLW(child, GetSubsystem(), name);
+ registry.AddChild(this, child);
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
new file mode 100644
index 0000000..1279d66
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitCommand.h"
+
+using namespace frc2;
+
+WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
+ auto durationStr = std::to_string(duration.to<double>());
+ SetName(wpi::Twine(GetName()) + ": " + wpi::Twine(durationStr) + " seconds");
+}
+
+void WaitCommand::Initialize() {
+ m_timer.Reset();
+ m_timer.Start();
+}
+
+void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
+
+bool WaitCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
new file mode 100644
index 0000000..739a049
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitUntilCommand.h"
+
+#include "frc2/Timer.h"
+
+using namespace frc2;
+
+WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
+ : m_condition{std::move(condition)} {}
+
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
+ : m_condition{[=] { return Timer::GetMatchTime() - time > 0_s; }} {}
+
+bool WaitUntilCommand::IsFinished() { return m_condition(); }
+
+bool WaitUntilCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
new file mode 100644
index 0000000..440675f
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Button.h"
+
+using namespace frc2;
+
+Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
+
+Button Button::WhenPressed(Command* command, bool interruptible) {
+ WhenActive(command, interruptible);
+ return *this;
+}
+
+Button Button::WhenPressed(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ WhenActive(std::move(toRun), requirements);
+ return *this;
+}
+
+Button Button::WhileHeld(Command* command, bool interruptible) {
+ WhileActiveContinous(command, interruptible);
+ return *this;
+}
+
+Button Button::WhileHeld(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ WhileActiveContinous(std::move(toRun), requirements);
+ return *this;
+}
+
+Button Button::WhenHeld(Command* command, bool interruptible) {
+ WhileActiveOnce(command, interruptible);
+ return *this;
+}
+
+Button Button::WhenReleased(Command* command, bool interruptible) {
+ WhenInactive(command, interruptible);
+ return *this;
+}
+
+Button Button::WhenReleased(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ WhenInactive(std::move(toRun), requirements);
+ return *this;
+}
+
+Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
+ ToggleWhenActive(command, interruptible);
+ return *this;
+}
+
+Button Button::CancelWhenPressed(Command* command) {
+ CancelWhenActive(command);
+ return *this;
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
new file mode 100644
index 0000000..caf188e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Trigger.h"
+
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+
+Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
+
+Trigger Trigger::WhenActive(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+
+ return *this;
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ return WhenActive(InstantCommand(std::move(toRun), requirements));
+}
+
+Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::WhileActiveContinous(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
+}
+
+Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressedLast && !pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements) {
+ return WhenInactive(InstantCommand(std::move(toRun), requirements));
+}
+
+Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ if (command->IsScheduled()) {
+ command->Cancel();
+ } else {
+ command->Schedule(interruptible);
+ }
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::CancelWhenActive(Command* command) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
new file mode 100644
index 0000000..4695c26
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -0,0 +1,247 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <frc/ErrorBase.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/Demangle.h>
+#include <wpi/SmallSet.h>
+
+#include "frc2/command/Subsystem.h"
+
+namespace frc2 {
+
+template <typename T>
+std::string GetTypeName(const T& type) {
+ return wpi::Demangle(typeid(type).name());
+}
+
+class ParallelCommandGroup;
+class ParallelRaceGroup;
+class ParallelDeadlineGroup;
+class SequentialCommandGroup;
+class PerpetualCommand;
+class ProxyScheduleCommand;
+
+/**
+ * A state machine representing a complete action to be performed by the robot.
+ * Commands are run by the CommandScheduler, and can be composed into
+ * CommandGroups to allow users to build complicated multi-step actions without
+ * the need to roll the state machine logic themselves.
+ *
+ * <p>Commands are run synchronously from the main robot loop; no multithreading
+ * is used, unless specified explicitly from the command implementation.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ *
+ * @see CommandScheduler
+ * @see CommandHelper
+ */
+class Command : public frc::ErrorBase {
+ public:
+ Command() = default;
+ virtual ~Command();
+
+ Command(const Command&);
+ Command& operator=(const Command&);
+ Command(Command&&) = default;
+ Command& operator=(Command&&) = default;
+
+ /**
+ * The initial subroutine of a command. Called once when the command is
+ * initially scheduled.
+ */
+ virtual void Initialize();
+
+ /**
+ * The main body of a command. Called repeatedly while the command is
+ * scheduled.
+ */
+ virtual void Execute();
+
+ /**
+ * The action to take when the command ends. Called when either the command
+ * finishes normally, or when it interrupted/canceled.
+ *
+ * @param interrupted whether the command was interrupted/canceled
+ */
+ virtual void End(bool interrupted);
+
+ /**
+ * Whether the command has finished. Once a command finishes, the scheduler
+ * will call its end() method and un-schedule it.
+ *
+ * @return whether the command has finished.
+ */
+ virtual bool IsFinished() { return false; }
+
+ /**
+ * Specifies the set of subsystems used by this command. Two commands cannot
+ * use the same subsystem at the same time. If the command is scheduled as
+ * interruptible and another command is scheduled that shares a requirement,
+ * the command will be interrupted. Else, the command will not be scheduled.
+ * If no subsystems are required, return an empty set.
+ *
+ * <p>Note: it is recommended that user implementations contain the
+ * requirements as a field, and return that field here, rather than allocating
+ * a new set every time this is called.
+ *
+ * @return the set of subsystems that are required
+ */
+ virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
+
+ /**
+ * Decorates this command with a timeout. If the specified timeout is
+ * exceeded before the command finishes normally, the command will be
+ * interrupted and un-scheduled. Note that the timeout only applies to the
+ * command returned by this method; the calling command is not itself changed.
+ *
+ * @param duration the timeout duration
+ * @return the command with the timeout added
+ */
+ ParallelRaceGroup WithTimeout(units::second_t duration) &&;
+
+ /**
+ * Decorates this command with an interrupt condition. If the specified
+ * condition becomes true before the command finishes normally, the command
+ * will be interrupted and un-scheduled. Note that this only applies to the
+ * command returned by this method; the calling command is not itself changed.
+ *
+ * @param condition the interrupt condition
+ * @return the command with the interrupt condition added
+ */
+ ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
+
+ /**
+ * Decorates this command with a runnable to run before this command starts.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the required subsystems
+ * @return the decorated command
+ */
+ SequentialCommandGroup BeforeStarting(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {}) &&;
+
+ /**
+ * Decorates this command with a runnable to run after the command finishes.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the required subsystems
+ * @return the decorated command
+ */
+ SequentialCommandGroup AndThen(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {}) &&;
+
+ /**
+ * Decorates this command to run perpetually, ignoring its ordinary end
+ * conditions. The decorated command can still be interrupted or canceled.
+ *
+ * @return the decorated command
+ */
+ PerpetualCommand Perpetually() &&;
+
+ /**
+ * Decorates this command to run "by proxy" by wrapping it in a {@link
+ * ProxyScheduleCommand}. This is useful for "forking off" from command groups
+ * when the user does not wish to extend the command's requirements to the
+ * entire command group.
+ *
+ * @return the decorated command
+ */
+ ProxyScheduleCommand AsProxy();
+
+ /**
+ * Schedules this command.
+ *
+ * @param interruptible whether this command can be interrupted by another
+ * command that shares one of its requirements
+ */
+ void Schedule(bool interruptible);
+
+ /**
+ * Schedules this command, defaulting to interruptible.
+ */
+ void Schedule() { Schedule(true); }
+
+ /**
+ * Cancels this command. Will call the command's interrupted() method.
+ * Commands will be canceled even if they are not marked as interruptible.
+ */
+ void Cancel();
+
+ /**
+ * Whether or not the command is currently scheduled. Note that this does not
+ * detect whether the command is being run by a CommandGroup, only whether it
+ * is directly being run by the scheduler.
+ *
+ * @return Whether the command is scheduled.
+ */
+ bool IsScheduled() const;
+
+ /**
+ * Whether the command requires a given subsystem. Named "hasRequirement"
+ * rather than "requires" to avoid confusion with
+ * {@link
+ * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
+ * - this may be able to be changed in a few years.
+ *
+ * @param requirement the subsystem to inquire about
+ * @return whether the subsystem is required
+ */
+ bool HasRequirement(Subsystem* requirement) const;
+
+ /**
+ * Whether the command is currently grouped in a command group. Used as extra
+ * insurance to prevent accidental independent use of grouped commands.
+ */
+ bool IsGrouped() const;
+
+ /**
+ * Sets whether the command is currently grouped in a command group. Can be
+ * used to "reclaim" a command if a group is no longer going to use it. NOT
+ * ADVISED!
+ */
+ void SetGrouped(bool grouped);
+
+ /**
+ * Whether the given command should run when the robot is disabled. Override
+ * to return true if the command should run when disabled.
+ *
+ * @return whether the command should run when the robot is disabled
+ */
+ virtual bool RunsWhenDisabled() const { return false; }
+
+ virtual std::string GetName() const;
+
+ protected:
+ /**
+ * Transfers ownership of this command to a unique pointer. Used for
+ * decorator methods.
+ */
+ virtual std::unique_ptr<Command> TransferOwnership() && = 0;
+
+ bool m_isGrouped = false;
+};
+
+/**
+ * Checks if two commands have disjoint requirement sets.
+ *
+ * @param first The first command to check.
+ * @param second The second command to check.
+ * @return False if first and second share a requirement.
+ */
+bool RequirementsDisjoint(Command* first, Command* second);
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
new file mode 100644
index 0000000..81af2f7
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <initializer_list>
+#include <string>
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+#include "frc2/command/Command.h"
+
+namespace frc2 {
+/**
+ * A Sendable base class for Commands.
+ */
+class CommandBase : public Command,
+ public frc::Sendable,
+ public frc::SendableHelper<CommandBase> {
+ public:
+ /**
+ * Adds the specified requirements to the command.
+ *
+ * @param requirements the requirements to add
+ */
+ void AddRequirements(std::initializer_list<Subsystem*> requirements);
+
+ void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
+
+ wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
+
+ /**
+ * Sets the name of this Command.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the name of this Command.
+ *
+ * @return Name
+ */
+ std::string GetName() const override;
+
+ /**
+ * 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);
+
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ protected:
+ CommandBase();
+ wpi::SmallSet<Subsystem*, 4> m_requirements;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
new file mode 100644
index 0000000..8b04b4c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <initializer_list>
+#include <memory>
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
+
+namespace frc2 {
+
+/**
+ * A base for CommandGroups. Statically tracks commands that have been
+ * allocated to groups to ensure those commands are not also used independently,
+ * which can result in inconsistent command state and unpredictable execution.
+ */
+class CommandGroupBase : public CommandBase {
+ public:
+ /**
+ * Requires that the specified command not have been already allocated to a
+ * CommandGroup. Reports an error if the command is already grouped.
+ *
+ * @param commands The command to check
+ * @return True if all the command is ungrouped.
+ */
+ static bool RequireUngrouped(Command& command);
+
+ /**
+ * Requires that the specified commands not have been already allocated to a
+ * CommandGroup. Reports an error if any of the commands are already grouped.
+ *
+ * @param commands The commands to check
+ * @return True if all the commands are ungrouped.
+ */
+ static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
+
+ /**
+ * Requires that the specified commands not have been already allocated to a
+ * CommandGroup. Reports an error if any of the commands are already grouped.
+ *
+ * @param commands The commands to check
+ * @return True if all the commands are ungrouped.
+ */
+ static bool RequireUngrouped(std::initializer_list<Command*>);
+
+ /**
+ * Adds the given commands to the command group.
+ *
+ * @param commands The commands to add.
+ */
+ virtual void AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) = 0;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
new file mode 100644
index 0000000..f78a848
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "frc2/command/Command.h"
+
+namespace frc2 {
+
+/**
+ * CRTP implementation to allow polymorphic decorator functions in Command.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ */
+template <typename Base, typename CRTP,
+ typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
+class CommandHelper : public Base {
+ using Base::Base;
+
+ public:
+ CommandHelper() = default;
+
+ protected:
+ std::unique_ptr<Command> TransferOwnership() && override {
+ return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
+ }
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
new file mode 100644
index 0000000..c114d6a
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -0,0 +1,340 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <initializer_list>
+#include <memory>
+#include <utility>
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/FunctionExtras.h>
+
+namespace frc2 {
+class Command;
+class Subsystem;
+
+/**
+ * The scheduler responsible for running Commands. A Command-based robot should
+ * call Run() on the singleton instance in its periodic block in order to run
+ * commands synchronously from the main loop. Subsystems should be registered
+ * with the scheduler using RegisterSubsystem() in order for their Periodic()
+ * methods to be called and for their default commands to be scheduled.
+ */
+class CommandScheduler final : public frc::Sendable,
+ public frc::ErrorBase,
+ public frc::SendableHelper<CommandScheduler> {
+ public:
+ /**
+ * Returns the Scheduler instance.
+ *
+ * @return the instance
+ */
+ static CommandScheduler& GetInstance();
+
+ ~CommandScheduler();
+ CommandScheduler(const CommandScheduler&) = delete;
+ CommandScheduler& operator=(const CommandScheduler&) = delete;
+
+ using Action = std::function<void(const Command&)>;
+
+ /**
+ * Adds a button binding to the scheduler, which will be polled to schedule
+ * commands.
+ *
+ * @param button The button to add
+ */
+ void AddButton(wpi::unique_function<void()> button);
+
+ /**
+ * Removes all button bindings from the scheduler.
+ */
+ void ClearButtons();
+
+ /**
+ * Schedules a command for execution. Does nothing if the command is already
+ * scheduled. If a command's requirements are not available, it will only be
+ * started if all the commands currently using those requirements have been
+ * scheduled as interruptible. If this is the case, they will be interrupted
+ * and the command will be scheduled.
+ *
+ * @param interruptible whether this command can be interrupted
+ * @param command the command to schedule
+ */
+ void Schedule(bool interruptible, Command* command);
+
+ /**
+ * Schedules a command for execution, with interruptible defaulted to true.
+ * Does nothing if the command is already scheduled.
+ *
+ * @param command the command to schedule
+ */
+ void Schedule(Command* command);
+
+ /**
+ * Schedules multiple commands for execution. Does nothing if the command is
+ * already scheduled. If a command's requirements are not available, it will
+ * only be started if all the commands currently using those requirements have
+ * been scheduled as interruptible. If this is the case, they will be
+ * interrupted and the command will be scheduled.
+ *
+ * @param interruptible whether the commands should be interruptible
+ * @param commands the commands to schedule
+ */
+ void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
+
+ /**
+ * Schedules multiple commands for execution. Does nothing if the command is
+ * already scheduled. If a command's requirements are not available, it will
+ * only be started if all the commands currently using those requirements have
+ * been scheduled as interruptible. If this is the case, they will be
+ * interrupted and the command will be scheduled.
+ *
+ * @param interruptible whether the commands should be interruptible
+ * @param commands the commands to schedule
+ */
+ void Schedule(bool interruptible, std::initializer_list<Command*> commands);
+
+ /**
+ * Schedules multiple commands for execution, with interruptible defaulted to
+ * true. Does nothing if the command is already scheduled.
+ *
+ * @param commands the commands to schedule
+ */
+ void Schedule(wpi::ArrayRef<Command*> commands);
+
+ /**
+ * Schedules multiple commands for execution, with interruptible defaulted to
+ * true. Does nothing if the command is already scheduled.
+ *
+ * @param commands the commands to schedule
+ */
+ void Schedule(std::initializer_list<Command*> commands);
+
+ /**
+ * Runs a single iteration of the scheduler. The execution occurs in the
+ * following order:
+ *
+ * <p>Subsystem periodic methods are called.
+ *
+ * <p>Button bindings are polled, and new commands are scheduled from them.
+ *
+ * <p>Currently-scheduled commands are executed.
+ *
+ * <p>End conditions are checked on currently-scheduled commands, and commands
+ * that are finished have their end methods called and are removed.
+ *
+ * <p>Any subsystems not being used as requirements have their default methods
+ * started.
+ */
+ void Run();
+
+ /**
+ * Registers subsystems with the scheduler. This must be called for the
+ * subsystem's periodic block to run when the scheduler is run, and for the
+ * subsystem's default command to be scheduled. It is recommended to call
+ * this from the constructor of your subsystem implementations.
+ *
+ * @param subsystem the subsystem to register
+ */
+ void RegisterSubsystem(Subsystem* subsystem);
+
+ /**
+ * Un-registers subsystems with the scheduler. The subsystem will no longer
+ * have its periodic block called, and will not have its default command
+ * scheduled.
+ *
+ * @param subsystem the subsystem to un-register
+ */
+ void UnregisterSubsystem(Subsystem* subsystem);
+
+ void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+ void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+ /**
+ * Sets the default command for a subsystem. Registers that subsystem if it
+ * is not already registered. Default commands will run whenever there is no
+ * other command currently scheduled that requires the subsystem. Default
+ * commands should be written to never end (i.e. their IsFinished() method
+ * should return false), as they would simply be re-scheduled if they do.
+ * Default commands must also require their subsystem.
+ *
+ * @param subsystem the subsystem whose default command will be set
+ * @param defaultCommand the default command to associate with the subsystem
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
+ if (!defaultCommand.HasRequirement(subsystem)) {
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse, "Default commands must require their subsystem!");
+ return;
+ }
+ if (defaultCommand.IsFinished()) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Default commands should not end!");
+ return;
+ }
+ SetDefaultCommandImpl(subsystem,
+ std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(defaultCommand)));
+ }
+
+ /**
+ * Gets the default command associated with this subsystem. Null if this
+ * subsystem has no default command associated with it.
+ *
+ * @param subsystem the subsystem to inquire about
+ * @return the default command associated with the subsystem
+ */
+ Command* GetDefaultCommand(const Subsystem* subsystem) const;
+
+ /**
+ * Cancels a command. The scheduler will only call the interrupted method of
+ * a canceled command, not the end method (though the interrupted method may
+ * itself call the end method). Commands will be canceled even if they are
+ * not scheduled as interruptible.
+ *
+ * @param command the command to cancel
+ */
+ void Cancel(Command* command);
+
+ /**
+ * Cancels commands. The scheduler will only call the interrupted method of a
+ * canceled command, not the end method (though the interrupted method may
+ * itself call the end method). Commands will be canceled even if they are
+ * not scheduled as interruptible.
+ *
+ * @param commands the commands to cancel
+ */
+ void Cancel(wpi::ArrayRef<Command*> commands);
+
+ /**
+ * Cancels commands. The scheduler will only call the interrupted method of a
+ * canceled command, not the end method (though the interrupted method may
+ * itself call the end method). Commands will be canceled even if they are
+ * not scheduled as interruptible.
+ *
+ * @param commands the commands to cancel
+ */
+ void Cancel(std::initializer_list<Command*> commands);
+
+ /**
+ * Cancels all commands that are currently scheduled.
+ */
+ void CancelAll();
+
+ /**
+ * Returns the time since a given command was scheduled. Note that this only
+ * works on commands that are directly scheduled by the scheduler; it will not
+ * work on commands inside of commandgroups, as the scheduler does not see
+ * them.
+ *
+ * @param command the command to query
+ * @return the time since the command was scheduled, in seconds
+ */
+ double TimeSinceScheduled(const Command* command) const;
+
+ /**
+ * Whether the given commands are running. Note that this only works on
+ * commands that are directly scheduled by the scheduler; it will not work on
+ * commands inside of CommandGroups, as the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
+
+ /**
+ * Whether the given commands are running. Note that this only works on
+ * commands that are directly scheduled by the scheduler; it will not work on
+ * commands inside of CommandGroups, as the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ bool IsScheduled(std::initializer_list<const Command*> commands) const;
+
+ /**
+ * Whether a given command is running. Note that this only works on commands
+ * that are directly scheduled by the scheduler; it will not work on commands
+ * inside of CommandGroups, as the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ bool IsScheduled(const Command* command) const;
+
+ /**
+ * Returns the command currently requiring a given subsystem. Null if no
+ * command is currently requiring the subsystem
+ *
+ * @param subsystem the subsystem to be inquired about
+ * @return the command currently requiring the subsystem
+ */
+ Command* Requiring(const Subsystem* subsystem) const;
+
+ /**
+ * Disables the command scheduler.
+ */
+ void Disable();
+
+ /**
+ * Enables the command scheduler.
+ */
+ void Enable();
+
+ /**
+ * Adds an action to perform on the initialization of any command by the
+ * scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandInitialize(Action action);
+
+ /**
+ * Adds an action to perform on the execution of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandExecute(Action action);
+
+ /**
+ * Adds an action to perform on the interruption of any command by the
+ * scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandInterrupt(Action action);
+
+ /**
+ * Adds an action to perform on the finishing of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandFinish(Action action);
+
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ private:
+ // Constructor; private as this is a singleton
+ CommandScheduler();
+
+ void SetDefaultCommandImpl(Subsystem* subsystem,
+ std::unique_ptr<Command> command);
+
+ class Impl;
+ std::unique_ptr<Impl> m_impl;
+
+ friend class CommandTestBase;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
new file mode 100644
index 0000000..41fc5d0
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc2 {
+/**
+ * Class that holds scheduling state for a command. Used internally by the
+ * CommandScheduler
+ */
+class CommandState final {
+ public:
+ CommandState() = default;
+
+ explicit CommandState(bool interruptible);
+
+ bool IsInterruptible() const { return m_interruptible; }
+
+ // The time since this command was initialized.
+ double TimeSinceInitialized() const;
+
+ private:
+ double m_startTime = -1;
+ bool m_interruptible;
+
+ void StartTiming();
+ void StartRunning();
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
new file mode 100644
index 0000000..af9f113
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * Runs one of two commands, depending on the value of the given condition when
+ * this command is initialized. Does not actually schedule the selected command
+ * - rather, the command is run through this command; this ensures that the
+ * command will behave as expected if used as part of a CommandGroup. Requires
+ * the requirements of both commands, again to ensure proper functioning when
+ * used in a CommandGroup. If this is undesired, consider using
+ * ScheduleCommand.
+ *
+ * <p>As this command contains multiple component commands within it, it is
+ * technically a command group; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ *
+ * @see ScheduleCommand
+ */
+class ConditionalCommand
+ : public CommandHelper<CommandBase, ConditionalCommand> {
+ public:
+ /**
+ * Creates a new ConditionalCommand.
+ *
+ * @param onTrue the command to run if the condition is true
+ * @param onFalse the command to run if the condition is false
+ * @param condition the condition to determine which command to run
+ */
+ template <class T1, class T2,
+ typename = std::enable_if_t<
+ std::is_base_of_v<Command, std::remove_reference_t<T1>>>,
+ typename = std::enable_if_t<
+ std::is_base_of_v<Command, std::remove_reference_t<T2>>>>
+ ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
+ : ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
+ std::forward<T1>(onTrue)),
+ std::make_unique<std::remove_reference_t<T2>>(
+ std::forward<T2>(onFalse)),
+ condition) {}
+
+ /**
+ * Creates a new ConditionalCommand.
+ *
+ * @param onTrue the command to run if the condition is true
+ * @param onFalse the command to run if the condition is false
+ * @param condition the condition to determine which command to run
+ */
+ ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+ std::unique_ptr<Command>&& onFalse,
+ std::function<bool()> condition);
+
+ ConditionalCommand(ConditionalCommand&& other) = default;
+
+ // No copy constructors for command groups
+ ConditionalCommand(const ConditionalCommand& other) = delete;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ std::unique_ptr<Command> m_onTrue;
+ std::unique_ptr<Command> m_onFalse;
+ std::function<bool()> m_condition;
+ Command* m_selectedCommand{nullptr};
+ bool m_runsWhenDisabled = true;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
new file mode 100644
index 0000000..f85f9be
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that allows the user to pass in functions for each of the basic
+ * command methods through the constructor. Useful for inline definitions of
+ * complex commands - note, however, that if a command is beyond a certain
+ * complexity it is usually better practice to write a proper class for it than
+ * to inline it.
+ */
+class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
+ public:
+ /**
+ * Creates a new FunctionalCommand.
+ *
+ * @param onInit the function to run on command initialization
+ * @param onExecute the function to run on command execution
+ * @param onEnd the function to run on command end
+ * @param isFinished the function that determines whether the command has
+ * finished
+ * @param requirements the subsystems required by this command
+ */
+ FunctionalCommand(std::function<void()> onInit,
+ std::function<void()> onExecute,
+ std::function<void(bool)> onEnd,
+ std::function<bool()> isFinished,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ FunctionalCommand(FunctionalCommand&& other) = default;
+
+ FunctionalCommand(const FunctionalCommand& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ std::function<void()> m_onInit;
+ std::function<void()> m_onExecute;
+ std::function<void(bool)> m_onEnd;
+ std::function<bool()> m_isFinished;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
new file mode 100644
index 0000000..97b4da2
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A Command that runs instantly; it will initialize, execute once, and end on
+ * the same iteration of the scheduler. Users can either pass in a Runnable and
+ * a set of requirements, or else subclass this command if desired.
+ */
+class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
+ public:
+ /**
+ * Creates a new InstantCommand that runs the given Runnable with the given
+ * requirements.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems required by this command
+ */
+ InstantCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ InstantCommand(InstantCommand&& other) = default;
+
+ InstantCommand(const InstantCommand& other) = default;
+
+ /**
+ * Creates a new InstantCommand with a Runnable that does nothing. Useful
+ * only as a no-arg constructor to call implicitly from subclass constructors.
+ */
+ InstantCommand();
+
+ void Initialize() override;
+
+ bool IsFinished() final;
+
+ private:
+ std::function<void()> m_toRun;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
new file mode 100644
index 0000000..e5a22a5
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+#include <functional>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a mecanum drive.
+ *
+ * <p>The command handles trajectory-following,
+ * Velocity PID calculations, and feedforwards internally. This
+ * is intended to be a more-or-less "complete solution" that can be used by
+ * teams without a great deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw wheel speeds from the PID controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the
+ * trajectory.
+ */
+class MecanumControllerCommand
+ : public CommandHelper<CommandBase, MecanumControllerCommand> {
+ public:
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow
+ * the provided trajectory. PID control and feedforward are handled
+ * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+ * motor.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon
+ * completion of the path this is left to the user, since it is not
+ * appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the
+ * final pose in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose,
+ * provided by the odometry class.
+ * @param feedforward The feedforward to use for the drivetrain.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+ * @param frontLeftController The front left wheel velocity PID.
+ * @param rearLeftController The rear left wheel velocity PID.
+ * @param frontRightController The front right wheel velocity PID.
+ * @param rearRightController The rear right wheel velocity PID.
+ * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
+ * the current wheel speeds.
+ * @param output The output of the velocity PIDs.
+ * @param requirements The subsystems to require.
+ */
+ MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+ frc2::PIDController frontLeftController,
+ frc2::PIDController rearLeftController,
+ frc2::PIDController frontRightController,
+ frc2::PIDController rearRightController,
+ std::function<void(units::volt_t, units::volt_t, units::volt_t,
+ units::volt_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Constructs a new MecanumControllerCommand that when executed will follow
+ * the provided trajectory. The user should implement a velocity PID on the
+ * desired output wheel velocities.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon
+ * completion of the path - this is left to the user, since it is not
+ * appropriate for paths with non-stationary end-states.
+ *
+ * <p>Note2: The rotation controller will calculate the rotation based on the
+ * final pose in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one
+ * of the odometry classes to provide this.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+ * @param output The output of the position PIDs.
+ * @param requirements The subsystems to require.
+ */
+ MecanumControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+ frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ units::meters_per_second_t maxWheelVelocity,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+ units::meters_per_second_t,
+ units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::Trajectory m_trajectory;
+ std::function<frc::Pose2d()> m_pose;
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+ frc::MecanumDriveKinematics m_kinematics;
+ std::unique_ptr<frc2::PIDController> m_xController;
+ std::unique_ptr<frc2::PIDController> m_yController;
+ std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+ const units::meters_per_second_t m_maxWheelVelocity;
+ std::unique_ptr<frc2::PIDController> m_frontLeftController;
+ std::unique_ptr<frc2::PIDController> m_rearLeftController;
+ std::unique_ptr<frc2::PIDController> m_frontRightController;
+ std::unique_ptr<frc2::PIDController> m_rearRightController;
+ std::function<frc::MecanumDriveWheelSpeeds()> m_currentWheelSpeeds;
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+ units::meters_per_second_t, units::meters_per_second_t)>
+ m_outputVel;
+ std::function<void(units::volt_t, units::volt_t, units::volt_t,
+ units::volt_t)>
+ m_outputVolts;
+
+ bool m_usePID;
+ frc2::Timer m_timer;
+ frc::MecanumDriveWheelSpeeds m_prevSpeeds;
+ units::second_t m_prevTime;
+ frc::Pose2d m_finalPose;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
new file mode 100644
index 0000000..847c693
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include <frc/Notifier.h>
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that starts a notifier to run the given runnable periodically in a
+ * separate thread. Has no end condition as-is; either subclass it or use {@link
+ * Command#withTimeout(double)} or
+ * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ *
+ * <p>WARNING: Do not use this class unless you are confident in your ability to
+ * make the executed code thread-safe. If you do not know what "thread-safe"
+ * means, that is a good sign that you should not use this class.
+ */
+class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
+ public:
+ /**
+ * Creates a new NotifierCommand.
+ *
+ * @param toRun the runnable for the notifier to run
+ * @param period the period at which the notifier should run
+ * @param requirements the subsystems required by this command
+ */
+ NotifierCommand(std::function<void()> toRun, units::second_t period,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ NotifierCommand(NotifierCommand&& other);
+
+ NotifierCommand(const NotifierCommand& other);
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ private:
+ std::function<void()> m_toRun;
+ frc::Notifier m_notifier;
+ units::second_t m_period;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
new file mode 100644
index 0000000..1089fd1
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include <frc/controller/PIDController.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a PIDController. Runs forever by
+ * default - to add exit conditions and/or other behavior, subclass this class.
+ * The controller calculation and output are performed synchronously in the
+ * command's execute() method.
+ *
+ * @see PIDController
+ */
+class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
+ public:
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * PIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpointSource the controller's reference (aka setpoint)
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ std::function<double()> setpointSource,
+ std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * PIDController with a constant setpoint.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpoint the controller's setpoint (aka setpoint)
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ PIDCommand(PIDController controller,
+ std::function<double()> measurementSource, double setpoint,
+ std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ PIDCommand(PIDCommand&& other) = default;
+
+ PIDCommand(const PIDCommand& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ /**
+ * Returns the PIDController used by the command.
+ *
+ * @return The PIDController
+ */
+ PIDController& GetController();
+
+ protected:
+ PIDController m_controller;
+ std::function<double()> m_measurement;
+ std::function<double()> m_setpoint;
+ std::function<void(double)> m_useOutput;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
new file mode 100644
index 0000000..62389c3
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/controller/PIDController.h>
+
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a PIDController to control an output. The controller
+ * is run synchronously from the subsystem's periodic() method.
+ *
+ * @see PIDController
+ */
+class PIDSubsystem : public SubsystemBase {
+ public:
+ /**
+ * Creates a new PIDSubsystem.
+ *
+ * @param controller the PIDController to use
+ */
+ explicit PIDSubsystem(PIDController controller);
+
+ void Periodic() override;
+
+ /**
+ * Sets the setpoint for the subsystem.
+ *
+ * @param setpoint the setpoint for the subsystem
+ */
+ void SetSetpoint(double setpoint);
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ virtual void Enable();
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ virtual void Disable();
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ bool IsEnabled();
+
+ /**
+ * Returns the PIDController.
+ *
+ * @return The controller.
+ */
+ PIDController& GetController();
+
+ protected:
+ PIDController m_controller;
+ bool m_enabled;
+
+ /**
+ * Returns the measurement of the process variable used by the PIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ virtual double GetMeasurement() = 0;
+
+ /**
+ * Uses the output from the PIDController.
+ *
+ * @param output the output of the PIDController
+ * @param setpoint the setpoint of the PIDController (for feedforward)
+ */
+ virtual void UseOutput(double output, double setpoint) = 0;
+
+ private:
+ double m_setpoint{0};
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
new file mode 100644
index 0000000..5b0f0da
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when the last
+ * command ends.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelCommandGroup
+ : public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
+ public:
+ /**
+ * Creates a new ParallelCommandGroup. The given commands will be executed
+ * simultaneously. The command group will finish when the last command
+ * finishes. If the CommandGroup is interrupted, only the commands that are
+ * still running will be interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ explicit ParallelCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands);
+
+ /**
+ * Creates a new ParallelCommandGroup. The given commands will be executed
+ * simultaneously. The command group will finish when the last command
+ * finishes. If the CommandGroup is interrupted, only the commands that are
+ * still running will be interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit ParallelCommandGroup(Types&&... commands) {
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ ParallelCommandGroup(ParallelCommandGroup&& other) = default;
+
+ // No copy constructors for commandgroups
+ ParallelCommandGroup(const ParallelCommandGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ ParallelCommandGroup(ParallelCommandGroup&) = delete;
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+ std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
+ bool m_runWhenDisabled{true};
+ bool isRunning = false;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
new file mode 100644
index 0000000..4debe75
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending only when a
+ * specific command (the "deadline") ends, interrupting all other commands that
+ * are still running at that point.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelDeadlineGroup
+ : public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
+ public:
+ /**
+ * Creates a new ParallelDeadlineGroup. The given commands (including the
+ * deadline) will be executed simultaneously. The CommandGroup will finish
+ * when the deadline finishes, interrupting all other still-running commands.
+ * If the CommandGroup is interrupted, only the commands still running will be
+ * interrupted.
+ *
+ * @param deadline the command that determines when the group ends
+ * @param commands the commands to be executed
+ */
+ ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
+ std::vector<std::unique_ptr<Command>>&& commands);
+ /**
+ * Creates a new ParallelDeadlineGroup. The given commands (including the
+ * deadline) will be executed simultaneously. The CommandGroup will finish
+ * when the deadline finishes, interrupting all other still-running commands.
+ * If the CommandGroup is interrupted, only the commands still running will be
+ * interrupted.
+ *
+ * @param deadline the command that determines when the group ends
+ * @param commands the commands to be executed
+ */
+ template <class T, class... Types,
+ typename = std::enable_if_t<
+ std::is_base_of_v<Command, std::remove_reference_t<T>>>,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
+ SetDeadline(std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(deadline)));
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
+
+ // No copy constructors for command groups
+ ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+ void SetDeadline(std::unique_ptr<Command>&& deadline);
+
+ std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
+ Command* m_deadline;
+ bool m_runWhenDisabled{true};
+ bool m_finished{true};
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
new file mode 100644
index 0000000..167dee4
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when any one
+ * of the commands ends and interrupting all the others.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelRaceGroup
+ : public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
+ public:
+ /**
+ * Creates a new ParallelCommandRace. The given commands will be executed
+ * simultaneously, and will "race to the finish" - the first command to finish
+ * ends the entire command, with all other commands being interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit ParallelRaceGroup(Types&&... commands) {
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ ParallelRaceGroup(ParallelRaceGroup&& other) = default;
+
+ // No copy constructors for command groups
+ ParallelRaceGroup(const ParallelRaceGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ ParallelRaceGroup(ParallelRaceGroup&) = delete;
+
+ template <class... Types>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+ std::vector<std::unique_ptr<Command>> m_commands;
+ bool m_runWhenDisabled{true};
+ bool m_finished{false};
+ bool isRunning = false;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
new file mode 100644
index 0000000..4a93704
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs another command in perpetuity, ignoring that command's
+ * end conditions. While this class does not extend {@link CommandGroupBase},
+ * it is still considered a CommandGroup, as it allows one to compose another
+ * command within it; the command instances that are passed to it cannot be
+ * added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
+ public:
+ /**
+ * Creates a new PerpetualCommand. Will run another command in perpetuity,
+ * ignoring that command's end conditions, unless this command itself is
+ * interrupted.
+ *
+ * @param command the command to run perpetually
+ */
+ explicit PerpetualCommand(std::unique_ptr<Command>&& command);
+
+ /**
+ * Creates a new PerpetualCommand. Will run another command in perpetuity,
+ * ignoring that command's end conditions, unless this command itself is
+ * interrupted.
+ *
+ * @param command the command to run perpetually
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ explicit PerpetualCommand(T&& command)
+ : PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command))) {}
+
+ PerpetualCommand(PerpetualCommand&& other) = default;
+
+ // No copy constructors for command groups
+ PerpetualCommand(const PerpetualCommand& other) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ PerpetualCommand(PerpetualCommand&) = delete;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ private:
+ std::unique_ptr<Command> m_command;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
new file mode 100644
index 0000000..7b706a0
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/InstantCommand.h"
+
+namespace frc2 {
+/**
+ * A command that prints a string when initialized.
+ */
+class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
+ public:
+ /**
+ * Creates a new a PrintCommand.
+ *
+ * @param message the message to print
+ */
+ explicit PrintCommand(const wpi::Twine& message);
+
+ PrintCommand(PrintCommand&& other) = default;
+
+ PrintCommand(const PrintCommand& other) = default;
+
+ bool RunsWhenDisabled() const override;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
new file mode 100644
index 0000000..49575de
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+#include <utility>
+
+#include <frc/controller/ProfiledPIDController.h>
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a ProfiledPIDController. Runs forever
+ * by default - to add exit conditions and/or other behavior, subclass this
+ * class. The controller calculation and output are performed synchronously in
+ * the command's execute() method.
+ *
+ * @see ProfiledPIDController<Distance>
+ */
+template <class Distance>
+class ProfiledPIDCommand
+ : public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource,
+ std::function<State()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements = {})
+ : m_controller{controller},
+ m_measurement{std::move(measurementSource)},
+ m_goal{std::move(goalSource)},
+ m_useOutput{std::move(useOutput)} {
+ this->AddRequirements(requirements);
+ }
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource,
+ std::function<Distance_t()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [&goalSource]() {
+ return State{goalSource(), Velocity_t{0}};
+ },
+ useOutput, requirements) {}
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController with a constant goal.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource, State goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [goal] { return goal; }, useOutput, requirements) {}
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController with a constant goal.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
+ std::function<Distance_t()> measurementSource,
+ Distance_t goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [goal] { return goal; }, useOutput, requirements) {}
+
+ ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
+
+ ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
+
+ void Initialize() override { m_controller.Reset(); }
+
+ void Execute() override {
+ m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
+ m_controller.GetSetpoint());
+ }
+
+ void End(bool interrupted) override {
+ m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
+ }
+
+ /**
+ * Returns the ProfiledPIDController used by the command.
+ *
+ * @return The ProfiledPIDController
+ */
+ frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
+
+ protected:
+ frc::ProfiledPIDController<Distance> m_controller;
+ std::function<Distance_t()> m_measurement;
+ std::function<State()> m_goal;
+ std::function<void(double, State)> m_useOutput;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
new file mode 100644
index 0000000..d5302c5
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/controller/ProfiledPIDController.h>
+#include <units/units.h>
+
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a ProfiledPIDController to control an output. The
+ * controller is run synchronously from the subsystem's periodic() method.
+ *
+ * @see ProfiledPIDController
+ */
+template <class Distance>
+class ProfiledPIDSubsystem : public SubsystemBase {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+ /**
+ * Creates a new ProfiledPIDSubsystem.
+ *
+ * @param controller the ProfiledPIDController to use
+ */
+ explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
+ : m_controller{controller} {}
+
+ void Periodic() override {
+ if (m_enabled) {
+ UseOutput(m_controller.Calculate(GetMeasurement(), m_goal),
+ m_controller.GetSetpoint());
+ }
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ void SetGoal(State goal) { m_goal = goal; }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ virtual void Enable() {
+ m_controller.Reset();
+ m_enabled = true;
+ }
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ virtual void Disable() {
+ UseOutput(0, State{Distance_t(0), Velocity_t(0)});
+ m_enabled = false;
+ }
+
+ /**
+ * Returns whether the controller is enabled.
+ *
+ * @return Whether the controller is enabled.
+ */
+ bool IsEnabled() { return m_enabled; }
+
+ /**
+ * Returns the ProfiledPIDController.
+ *
+ * @return The controller.
+ */
+ frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
+
+ protected:
+ frc::ProfiledPIDController<Distance> m_controller;
+ bool m_enabled{false};
+
+ /**
+ * Returns the measurement of the process variable used by the
+ * ProfiledPIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ virtual Distance_t GetMeasurement() = 0;
+
+ /**
+ * Uses the output from the ProfiledPIDController.
+ *
+ * @param output the output of the ProfiledPIDController
+ * @param setpoint the setpoint state of the ProfiledPIDController, for
+ * feedforward
+ */
+ virtual void UseOutput(double output, State setpoint) = 0;
+
+ private:
+ State m_goal;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
new file mode 100644
index 0000000..ef5204f
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
+
+namespace frc2 {
+/**
+ * Schedules the given commands when this command is initialized, and ends when
+ * all the commands are no longer scheduled. Useful for forking off from
+ * CommandGroups. If this command is interrupted, it will cancel all of the
+ * commands.
+ */
+class ProxyScheduleCommand
+ : public CommandHelper<CommandBase, ProxyScheduleCommand> {
+ public:
+ /**
+ * Creates a new ProxyScheduleCommand that schedules the given commands when
+ * initialized, and ends when they are all no longer scheduled.
+ *
+ * @param toSchedule the commands to schedule
+ */
+ explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+ ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
+
+ ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ void Execute() override;
+
+ bool IsFinished() override;
+
+ private:
+ wpi::SmallVector<Command*, 4> m_toSchedule;
+ bool m_finished{false};
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
new file mode 100644
index 0000000..60d1b7e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that uses a RAMSETE controller to follow a trajectory
+ * with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and
+ * feedforwards internally. This is intended to be a more-or-less "complete
+ * solution" that can be used by teams without a great deal of controls
+ * expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw wheel speeds from the RAMSETE controller.
+ *
+ * @see RamseteController
+ * @see Trajectory
+ */
+class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
+ public:
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the
+ * provided trajectory. PID control and feedforward are handled internally,
+ * and outputs are scaled -12 to 12 representing units of volts.
+ *
+ * <p>Note: The controller will *not* set the outputVolts to zero upon
+ * completion of the path - this is left to the user, since it is not
+ * appropriate for paths with nonstationary endstates.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param controller The RAMSETE controller used to follow the
+ * trajectory.
+ * @param feedforward A component for calculating the feedforward for the
+ * drive.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param wheelSpeeds A function that supplies the speeds of the left
+ * and right sides of the robot drive.
+ * @param leftController The PIDController for the left side of the robot
+ * drive.
+ * @param rightController The PIDController for the right side of the robot
+ * drive.
+ * @param output A function that consumes the computed left and right
+ * outputs (in volts) for the robot drive.
+ * @param requirements The subsystems to require.
+ */
+ RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::SimpleMotorFeedforward<units::meters> feedforward,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
+ frc2::PIDController leftController,
+ frc2::PIDController rightController,
+ std::function<void(units::volt_t, units::volt_t)> output,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the
+ * provided trajectory. Performs no PID control and calculates no
+ * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+ * and will need to be converted into a usable form by the user.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param controller The RAMSETE controller used to follow the
+ * trajectory.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param output A function that consumes the computed left and right
+ * wheel speeds.
+ * @param requirements The subsystems to require.
+ */
+ RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<void(units::meters_per_second_t,
+ units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::Trajectory m_trajectory;
+ std::function<frc::Pose2d()> m_pose;
+ frc::RamseteController m_controller;
+ frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+ frc::DifferentialDriveKinematics m_kinematics;
+ std::function<frc::DifferentialDriveWheelSpeeds()> m_speeds;
+ std::unique_ptr<frc2::PIDController> m_leftController;
+ std::unique_ptr<frc2::PIDController> m_rightController;
+ std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+ m_outputVel;
+
+ Timer m_timer;
+ units::second_t m_prevTime;
+ frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
+ bool m_usePID;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
new file mode 100644
index 0000000..aae7b74
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a Runnable continuously. Has no end condition as-is;
+ * either subclass it or use Command.WithTimeout() or
+ * Command.WithInterrupt() to give it one. If you only wish
+ * to execute a Runnable once, use InstantCommand.
+ */
+class RunCommand : public CommandHelper<CommandBase, RunCommand> {
+ public:
+ /**
+ * Creates a new RunCommand. The Runnable will be run continuously until the
+ * command ends. Does not run when disabled.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems to require
+ */
+ RunCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ RunCommand(RunCommand&& other) = default;
+
+ RunCommand(const RunCommand& other) = default;
+
+ void Execute();
+
+ protected:
+ std::function<void()> m_toRun;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
new file mode 100644
index 0000000..c2824ca
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
+
+namespace frc2 {
+/**
+ * Schedules the given commands when this command is initialized. Useful for
+ * forking off from CommandGroups. Note that if run from a CommandGroup, the
+ * group will not know about the status of the scheduled commands, and will
+ * treat this command as finishing instantly.
+ */
+class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
+ public:
+ /**
+ * Creates a new ScheduleCommand that schedules the given commands when
+ * initialized.
+ *
+ * @param toSchedule the commands to schedule
+ */
+ explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+ ScheduleCommand(ScheduleCommand&& other) = default;
+
+ ScheduleCommand(const ScheduleCommand& other) = default;
+
+ void Initialize() override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ wpi::SmallVector<Command*, 4> m_toSchedule;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
new file mode 100644
index 0000000..e1074f4
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <type_traits>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/PrintCommand.h"
+
+namespace frc2 {
+/**
+ * Runs one of a selection of commands, either using a selector and a key to
+ * command mapping, or a supplier that returns the command directly at runtime.
+ * Does not actually schedule the selected command - rather, the command is run
+ * through this command; this ensures that the command will behave as expected
+ * if used as part of a CommandGroup. Requires the requirements of all included
+ * commands, again to ensure proper functioning when used in a CommandGroup. If
+ * this is undesired, consider using ScheduleCommand.
+ *
+ * <p>As this command contains multiple component commands within it, it is
+ * technically a command group; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+template <typename Key>
+class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
+ public:
+ /**
+ * Creates a new selectcommand.
+ *
+ * @param commands the map of commands to choose from
+ * @param selector the selector to determine which command to run
+ */
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ SelectCommand(std::function<Key()> selector,
+ std::pair<Key, Types>... commands)
+ : m_selector{std::move(selector)} {
+ std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
+
+ ((void)foo.emplace_back(commands.first,
+ std::make_unique<std::remove_reference_t<Types>>(
+ std::move(commands.second))),
+ ...);
+
+ for (auto&& command : foo) {
+ if (!CommandGroupBase::RequireUngrouped(command.second)) {
+ return;
+ }
+ }
+
+ for (auto&& command : foo) {
+ this->AddRequirements(command.second->GetRequirements());
+ m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+ m_commands.emplace(std::move(command.first), std::move(command.second));
+ }
+ }
+
+ SelectCommand(
+ std::function<Key()> selector,
+ std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
+ : m_selector{std::move(selector)} {
+ for (auto&& command : commands) {
+ if (!CommandGroupBase::RequireUngrouped(command.second)) {
+ return;
+ }
+ }
+
+ for (auto&& command : commands) {
+ this->AddRequirements(command.second->GetRequirements());
+ m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+ m_commands.emplace(std::move(command.first), std::move(command.second));
+ }
+ }
+
+ // No copy constructors for command groups
+ SelectCommand(const SelectCommand& other) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ SelectCommand(SelectCommand&) = delete;
+
+ /**
+ * Creates a new selectcommand.
+ *
+ * @param toRun a supplier providing the command to run
+ */
+ explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
+
+ SelectCommand(SelectCommand&& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override { m_selectedCommand->Execute(); }
+
+ void End(bool interrupted) override {
+ return m_selectedCommand->End(interrupted);
+ }
+
+ bool IsFinished() override { return m_selectedCommand->IsFinished(); }
+
+ bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ protected:
+ std::unique_ptr<Command> TransferOwnership() && override {
+ return std::make_unique<SelectCommand>(std::move(*this));
+ }
+
+ private:
+ std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
+ std::function<Key()> m_selector;
+ std::function<Command*()> m_toRun;
+ Command* m_selectedCommand;
+ bool m_runsWhenDisabled = true;
+};
+
+template <typename T>
+void SelectCommand<T>::Initialize() {
+ if (m_selector) {
+ auto find = m_commands.find(m_selector());
+ if (find == m_commands.end()) {
+ m_selectedCommand = new PrintCommand(
+ "SelectCommand selector value does not correspond to any command!");
+ return;
+ }
+ m_selectedCommand = find->second.get();
+ } else {
+ m_selectedCommand = m_toRun();
+ }
+ m_selectedCommand->Initialize();
+}
+
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
new file mode 100644
index 0000000..c49356e
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <limits>
+#include <memory>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+
+const size_t invalid_index = std::numeric_limits<size_t>::max();
+
+/**
+ * A CommandGroups that runs a list of commands in sequence.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class SequentialCommandGroup
+ : public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
+ public:
+ /**
+ * Creates a new SequentialCommandGroup. The given commands will be run
+ * sequentially, with the CommandGroup finishing when the last command
+ * finishes.
+ *
+ * @param commands the commands to include in this group.
+ */
+ explicit SequentialCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands);
+
+ /**
+ * Creates a new SequentialCommandGroup. The given commands will be run
+ * sequentially, with the CommandGroup finishing when the last command
+ * finishes.
+ *
+ * @param commands the commands to include in this group.
+ */
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit SequentialCommandGroup(Types&&... commands) {
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ SequentialCommandGroup(SequentialCommandGroup&& other) = default;
+
+ // No copy constructors for command groups
+ SequentialCommandGroup(const SequentialCommandGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ SequentialCommandGroup(SequentialCommandGroup&) = delete;
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
+
+ wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
+ size_t m_currentCommandIndex{invalid_index};
+ bool m_runWhenDisabled{true};
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h b/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
new file mode 100644
index 0000000..d21f572
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+namespace frc2 {
+template <typename T>
+void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
+ for (auto addCommand : toAdd) {
+ bool exists = false;
+ for (auto existingCommand : vector) {
+ if (addCommand == existingCommand) {
+ exists = true;
+ break;
+ }
+ }
+ if (!exists) {
+ vector.emplace_back(addCommand);
+ }
+ }
+}
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
new file mode 100644
index 0000000..32b7e76
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a given runnable when it is initalized, and another
+ * runnable when it ends. Useful for running and then stopping a motor, or
+ * extending and then retracting a solenoid. Has no end condition as-is; either
+ * subclass it or use Command.WithTimeout() or Command.WithInterrupt() to give
+ * it one.
+ */
+class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
+ public:
+ /**
+ * Creates a new StartEndCommand. Will run the given runnables when the
+ * command starts and when it ends.
+ *
+ * @param onInit the Runnable to run on command init
+ * @param onEnd the Runnable to run on command end
+ * @param requirements the subsystems required by this command
+ */
+ StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ StartEndCommand(StartEndCommand&& other) = default;
+
+ StartEndCommand(const StartEndCommand& other);
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ protected:
+ std::function<void()> m_onInit;
+ std::function<void()> m_onEnd;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
new file mode 100644
index 0000000..687510d
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A robot subsystem. Subsystems are the basic unit of robot organization in
+ * the Command-based framework; they encapsulate low-level hardware objects
+ * (motor controllers, sensors, etc) and provide methods through which they can
+ * be used by Commands. Subsystems are used by the CommandScheduler's resource
+ * management system to ensure multiple robot actions are not "fighting" over
+ * the same hardware; Commands that use a subsystem should include that
+ * subsystem in their GetRequirements() method, and resources used within a
+ * subsystem should generally remain encapsulated and not be shared by other
+ * parts of the robot.
+ *
+ * <p>Subsystems must be registered with the scheduler with the
+ * CommandScheduler.RegisterSubsystem() method in order for the
+ * Periodic() method to be called. It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations. The
+ * SubsystemBase class offers a simple base for user implementations
+ * that handles this.
+ *
+ * @see Command
+ * @see CommandScheduler
+ * @see SubsystemBase
+ */
+class Subsystem {
+ public:
+ ~Subsystem();
+ /**
+ * This method is called periodically by the CommandScheduler. Useful for
+ * updating subsystem-specific state that you don't want to offload to a
+ * Command. Teams should try to be consistent within their own codebases
+ * about which responsibilities will be handled by Commands, and which will be
+ * handled here.
+ */
+ virtual void Periodic();
+
+ /**
+ * Sets the default Command of the subsystem. The default command will be
+ * automatically scheduled when no other commands are scheduled that require
+ * the subsystem. Default commands should generally not end on their own, i.e.
+ * their IsFinished() method should always return false. Will automatically
+ * register this subsystem with the CommandScheduler.
+ *
+ * @param defaultCommand the default command to associate with this subsystem
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ void SetDefaultCommand(T&& defaultCommand) {
+ CommandScheduler::GetInstance().SetDefaultCommand(
+ this, std::forward<T>(defaultCommand));
+ }
+
+ /**
+ * Gets the default command for this subsystem. Returns null if no default
+ * command is currently associated with the subsystem.
+ *
+ * @return the default command associated with this subsystem
+ */
+ Command* GetDefaultCommand() const;
+
+ /**
+ * Returns the command currently running on this subsystem. Returns null if
+ * no command is currently scheduled that requires this subsystem.
+ *
+ * @return the scheduled command currently requiring this subsystem
+ */
+ Command* GetCurrentCommand() const;
+
+ /**
+ * Registers this subsystem with the CommandScheduler, allowing its
+ * Periodic() method to be called when the scheduler runs.
+ */
+ void Register();
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
new file mode 100644
index 0000000..d75ad0d
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include "frc2/command/Subsystem.h"
+
+namespace frc2 {
+/**
+ * A base for subsystems that handles registration in the constructor, and
+ * provides a more intuitive method for setting the default command.
+ */
+class SubsystemBase : public Subsystem,
+ public frc::Sendable,
+ public frc::SendableHelper<SubsystemBase> {
+ public:
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ /**
+ * 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& name);
+
+ /**
+ * Associate a Sendable with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ void AddChild(std::string name, frc::Sendable* child);
+
+ protected:
+ SubsystemBase();
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
new file mode 100644
index 0000000..ab3fe10
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+#include <functional>
+#include <memory>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/Trajectory.h>
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+
+/**
+ * A command that uses two PID controllers ({@link PIDController}) and a
+ * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
+ * {@link Trajectory} with a swerve drive.
+ *
+ * <p>The command handles trajectory-following, Velocity PID calculations, and
+ * feedforwards internally. This is intended to be a more-or-less "complete
+ * solution" that can be used by teams without a great deal of controls
+ * expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw module states from the position PID controllers.
+ *
+ * <p>The robot angle controller does not follow the angle given by
+ * the trajectory but rather goes to the angle given in the final state of the
+ * trajectory.
+ */
+template <size_t NumModules>
+class SwerveControllerCommand
+ : public CommandHelper<CommandBase, SwerveControllerCommand<NumModules>> {
+ using voltsecondspermeter =
+ units::compound_unit<units::voltage::volt, units::second,
+ units::inverse<units::meter>>;
+ using voltsecondssquaredpermeter =
+ units::compound_unit<units::voltage::volt, units::squared<units::second>,
+ units::inverse<units::meter>>;
+
+ public:
+ /**
+ * Constructs a new SwerveControllerCommand that when executed will follow the
+ * provided trajectory. This command will not return output voltages but
+ * rather raw module states from the position controllers which need to be put
+ * into a velocity PID.
+ *
+ * <p>Note: The controllers will *not* set the outputVolts to zero upon
+ * completion of the path- this is left to the user, since it is not
+ * appropriate for paths with nonstationary endstates.
+ *
+ * <p>Note 2: The rotation controller will calculate the rotation based on the
+ * final pose in the trajectory, not the poses at each time step.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose,
+ * provided by the odometry class.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param xController The Trajectory Tracker PID controller
+ * for the robot's x position.
+ * @param yController The Trajectory Tracker PID controller
+ * for the robot's y position.
+ * @param thetaController The Trajectory Tracker PID controller
+ * for angle for the robot.
+ * @param output The raw output module states from the
+ * position controllers.
+ * @param requirements The subsystems to require.
+ */
+ SwerveControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ frc2::PIDController xController, frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::Trajectory m_trajectory;
+ std::function<frc::Pose2d()> m_pose;
+ frc::SwerveDriveKinematics<NumModules> m_kinematics;
+ std::unique_ptr<frc2::PIDController> m_xController;
+ std::unique_ptr<frc2::PIDController> m_yController;
+ std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+ std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+ m_outputStates;
+
+ frc2::Timer m_timer;
+ units::second_t m_prevTime;
+ frc::Pose2d m_finalPose;
+};
+} // namespace frc2
+
+#include "SwerveControllerCommand.inc"
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
new file mode 100644
index 0000000..7beafef
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+namespace frc2 {
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ frc2::PIDController xController, frc2::PIDController yController,
+ frc::ProfiledPIDController<units::radians> thetaController,
+ std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_kinematics(kinematics),
+ m_xController(std::make_unique<frc2::PIDController>(xController)),
+ m_yController(std::make_unique<frc2::PIDController>(yController)),
+ m_thetaController(
+ std::make_unique<frc::ProfiledPIDController<units::radians>>(
+ thetaController)),
+ m_outputStates(output) {
+ this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::Initialize() {
+ m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
+
+ m_timer.Reset();
+ m_timer.Start();
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::Execute() {
+ auto curTime = units::second_t(m_timer.Get());
+
+ auto m_desiredState = m_trajectory.Sample(curTime);
+ auto m_desiredPose = m_desiredState.pose;
+
+ auto m_poseError = m_desiredPose.RelativeTo(m_pose());
+
+ auto targetXVel = units::meters_per_second_t(m_xController->Calculate(
+ (m_pose().Translation().X().template to<double>()),
+ (m_desiredPose.Translation().X().template to<double>())));
+ auto targetYVel = units::meters_per_second_t(m_yController->Calculate(
+ (m_pose().Translation().Y().template to<double>()),
+ (m_desiredPose.Translation().Y().template to<double>())));
+
+ // Profiled PID Controller only takes meters as setpoint and measurement
+ // The robot will go to the desired rotation of the final pose in the
+ // trajectory, not following the poses at individual states.
+ auto targetAngularVel =
+ units::radians_per_second_t(m_thetaController->Calculate(
+ m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
+
+ auto vRef = m_desiredState.velocity;
+
+ targetXVel += vRef * m_poseError.Rotation().Cos();
+ targetYVel += vRef * m_poseError.Rotation().Sin();
+
+ auto targetChassisSpeeds =
+ frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
+
+ auto targetModuleStates =
+ m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
+
+ m_outputStates(targetModuleStates);
+}
+
+template <size_t NumModules>
+void SwerveControllerCommand<NumModules>::End(bool interrupted) {
+ m_timer.Stop();
+}
+
+template <size_t NumModules>
+bool SwerveControllerCommand<NumModules>::IsFinished() {
+ return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
+
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
new file mode 100644
index 0000000..e21de07
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
+#include <frc/trajectory/TrapezoidProfile.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a TrapezoidProfile. Useful for smoothly controlling
+ * mechanism motion.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Distance>
+class TrapezoidProfileCommand
+ : public CommandHelper<CommandBase, TrapezoidProfileCommand<Distance>> {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+
+ public:
+ /**
+ * Creates a new TrapezoidProfileCommand that will execute the given
+ * TrapezoidalProfile. Output will be piped to the provided consumer function.
+ *
+ * @param profile The motion profile to execute.
+ * @param output The consumer for the profile output.
+ */
+ TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
+ std::function<void(State)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_profile(profile), m_output(output) {
+ this->AddRequirements(requirements);
+ }
+
+ void Initialize() override {
+ m_timer.Reset();
+ m_timer.Start();
+ }
+
+ void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); }
+
+ void End(bool interrupted) override { m_timer.Stop(); }
+
+ bool IsFinished() override {
+ return m_timer.HasPeriodPassed(m_profile.TotalTime());
+ }
+
+ private:
+ frc::TrapezoidProfile<Distance> m_profile;
+ std::function<void(State)> m_output;
+
+ Timer m_timer;
+};
+
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
new file mode 100644
index 0000000..a833f3c
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that generates and runs trapezoidal motion profiles
+ * automatically. The user specifies how to use the current state of the motion
+ * profile by overriding the `UseState` method.
+ */
+template <class Distance>
+class TrapezoidProfileSubsystem : public SubsystemBase {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using State = typename frc::TrapezoidProfile<Distance>::State;
+ using Constraints = typename frc::TrapezoidProfile<Distance>::Constraints;
+
+ public:
+ /**
+ * Creates a new TrapezoidProfileSubsystem.
+ *
+ * @param constraints The constraints (maximum velocity and acceleration)
+ * for the profiles.
+ * @param initialPosition The initial position of the controller mechanism
+ * when the subsystem is constructed.
+ * @param period The period of the main robot loop, in seconds.
+ */
+ TrapezoidProfileSubsystem(Constraints constraints, Distance_t position,
+ units::second_t period = 20_ms)
+ : m_constraints(constraints),
+ m_state{position, Velocity_t(0)},
+ m_goal{position, Velocity_t{0}},
+ m_period(period) {}
+
+ void Periodic() override {
+ auto profile =
+ frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
+ m_state = profile.Calculate(m_period);
+ UseState(m_state);
+ }
+
+ /**
+ * Sets the goal state for the subsystem.
+ *
+ * @param goal The goal state for the subsystem's motion profile.
+ */
+ void SetGoal(State goal) { m_goal = goal; }
+
+ /**
+ * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
+ *
+ * @param goal The goal position for the subsystem's motion profile.
+ */
+ void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
+
+ protected:
+ /**
+ * Users should override this to consume the current state of the motion
+ * profile.
+ *
+ * @param state The current state of the motion profile.
+ */
+ virtual void UseState(State state) = 0;
+
+ private:
+ Constraints m_constraints;
+ State m_state;
+ State m_goal;
+ units::second_t m_period;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
new file mode 100644
index 0000000..ab97958
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but takes a specified amount of time to finish.
+ * Useful for CommandGroups. Can also be subclassed to make a command with an
+ * internal timer.
+ */
+class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
+ public:
+ /**
+ * Creates a new WaitCommand. This command will do nothing, and end after the
+ * specified duration.
+ *
+ * @param duration the time to wait
+ */
+ explicit WaitCommand(units::second_t duration);
+
+ WaitCommand(WaitCommand&& other) = default;
+
+ WaitCommand(const WaitCommand& other) = default;
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ protected:
+ Timer m_timer;
+
+ private:
+ units::second_t m_duration;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
new file mode 100644
index 0000000..50b4855
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but ends after a specified match time or
+ * condition. Useful for CommandGroups.
+ */
+class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
+ public:
+ /**
+ * Creates a new WaitUntilCommand that ends after a given condition becomes
+ * true.
+ *
+ * @param condition the condition to determine when to end
+ */
+ explicit WaitUntilCommand(std::function<bool()> condition);
+
+ /**
+ * Creates a new WaitUntilCommand that ends after a given match time.
+ *
+ * <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this
+ * command does NOT guarantee that the time at which the action is performed
+ * will be judged to be legal by the referees. When in doubt, add a safety
+ * factor or time the action manually.
+ *
+ * @param time the match time after which to end, in seconds
+ */
+ explicit WaitUntilCommand(units::second_t time);
+
+ WaitUntilCommand(WaitUntilCommand&& other) = default;
+
+ WaitUntilCommand(const WaitUntilCommand& other) = default;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ std::function<bool()> m_condition;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
new file mode 100644
index 0000000..6561652
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -0,0 +1,213 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <utility>
+
+#include "Trigger.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to button presses. Can be composed
+ * with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class Button : public Trigger {
+ public:
+ /**
+ * Create a new button that is pressed when the given condition is true.
+ *
+ * @param isActive Whether the button is pressed.
+ */
+ explicit Button(std::function<bool()> isPressed);
+
+ /**
+ * Create a new button that is pressed active (default constructor) - activity
+ * can be further determined by subclass code.
+ */
+ Button() = default;
+
+ /**
+ * Binds a command to start when the button is pressed. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Button WhenPressed(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the button is pressed. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhenPressed(T&& command, bool interruptible = true) {
+ WhenActive(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the button is pressed.
+ *
+ * @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
+ */
+ Button WhenPressed(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Binds a command to be started repeatedly while the button is pressed, and
+ * cancelled when it is released. Takes a raw pointer, and so is non-owning;
+ * users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button WhileHeld(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started repeatedly while the button is pressed, and
+ * cancelled when it is released. Transfers command ownership to the button
+ * scheduler, so the user does not have to worry about lifespan - rvalue refs
+ * will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhileHeld(T&& command, bool interruptible = true) {
+ WhileActiveContinous(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute repeatedly while the button is pressed.
+ *
+ * @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
+ */
+ Button WhileHeld(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Binds a command to be started when the button is pressed, and cancelled
+ * when it is released. Takes a raw pointer, and so is non-owning; users are
+ * responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button WhenHeld(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started when the button is pressed, and cancelled
+ * when it is released. Transfers command ownership to the button scheduler,
+ * so the user does not have to worry about lifespan - rvalue refs will be
+ * *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhenHeld(T&& command, bool interruptible = true) {
+ WhileActiveOnce(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a command to start when the button is released. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button WhenReleased(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the button is pressed. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhenReleased(T&& command, bool interruptible = true) {
+ WhenInactive(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the button is released.
+ *
+ * @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
+ */
+ Button WhenReleased(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Binds a command to start when the button is pressed, and be cancelled when
+ * it is pressed again. Takes a raw pointer, and so is non-owning; users are
+ * responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button ToggleWhenPressed(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the button is pressed, and be cancelled when
+ * it is pessed again. Transfers command ownership to the button scheduler,
+ * so the user does not have to worry about lifespan - rvalue refs will be
+ * *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button ToggleWhenPressed(T&& command, bool interruptible = true) {
+ ToggleWhenActive(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a command to be cancelled when the button is pressed. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * and scheduling of the command.
+ *
+ * @param command The command to bind.
+ * @return The button, for chained calls.
+ */
+ Button CancelWhenPressed(Command* command);
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
new file mode 100644
index 0000000..a23738b
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick button presses. Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class JoystickButton : public Button {
+ public:
+ /**
+ * Creates a JoystickButton that commands can be bound to.
+ *
+ * @param joystick The joystick on which the button is located.
+ * @param buttonNumber The number of the button on the joystic.
+ */
+ explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
+ : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
+
+ bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
+
+ private:
+ frc::GenericHID* m_joystick;
+ int m_buttonNumber;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
new file mode 100644
index 0000000..758cab2
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick POV presses. Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class POVButton : public Button {
+ public:
+ /**
+ * Creates a POVButton that commands can be bound to.
+ *
+ * @param joystick The joystick on which the button is located.
+ * @param angle The angle of the POV corresponding to a button press.
+ * @param povNumber The number of the POV on the joystick.
+ */
+ POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
+ : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
+
+ bool Get() const override {
+ return m_joystick->GetPOV(m_povNumber) == m_angle;
+ }
+
+ private:
+ frc::GenericHID* m_joystick;
+ int m_angle;
+ int m_povNumber;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
new file mode 100644
index 0000000..efc4bc0
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -0,0 +1,332 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <memory>
+#include <utility>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to events. The
+ * Trigger class is a base for all command-event-binding classes, and so the
+ * methods are named fairly abstractly; for purpose-specific wrappers, see
+ * Button.
+ *
+ * @see Button
+ */
+class Trigger {
+ public:
+ /**
+ * Create a new trigger that is active when the given condition is true.
+ *
+ * @param isActive Whether the trigger is active.
+ */
+ explicit Trigger(std::function<bool()> isActive)
+ : m_isActive{std::move(isActive)} {}
+
+ /**
+ * Create a new trigger that is never active (default constructor) - activity
+ * can be further determined by subclass code.
+ */
+ Trigger() {
+ m_isActive = [] { return false; };
+ }
+
+ Trigger(const Trigger& other);
+
+ /**
+ * Returns whether the trigger is active. Can be overridden by a subclass.
+ *
+ * @return Whether the trigger is active.
+ */
+ virtual bool Get() const { return m_isActive(); }
+
+ /**
+ * Binds a command to start when the trigger becomes active. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhenActive(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the trigger becomes active. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhenActive(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the trigger becomes active.
+ *
+ * @param toRun the runnable to execute.
+ * @paaram requirements the required subsystems.
+ */
+ Trigger WhenActive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Binds a command to be started repeatedly while the trigger is active, and
+ * cancelled when it becomes inactive. Takes a raw pointer, and so is
+ * non-owning; users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhileActiveContinous(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started repeatedly while the trigger is active, and
+ * cancelled when it becomes inactive. Transfers command ownership to the
+ * button scheduler, so the user does not have to worry about lifespan -
+ * rvalue refs will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute repeatedly while the trigger is active.
+ *
+ * @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
+ */
+ Trigger WhileActiveContinous(
+ std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Binds a command to be started when the trigger becomes active, and
+ * cancelled when it becomes inactive. Takes a raw pointer, and so is
+ * non-owning; users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhileActiveOnce(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started when the trigger becomes active, and
+ * cancelled when it becomes inactive. Transfers command ownership to the
+ * button scheduler, so the user does not have to worry about lifespan -
+ * rvalue refs will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a command to start when the trigger becomes inactive. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhenInactive(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the trigger becomes inactive. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhenInactive(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressedLast && !pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the trigger becomes inactive.
+ *
+ * @param toRun the runnable to execute.
+ * @param requirements the required subsystems.
+ */
+ Trigger WhenInactive(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements = {});
+
+ /**
+ * Binds a command to start when the trigger becomes active, and be cancelled
+ * when it again becomes active. Takes a raw pointer, and so is non-owning;
+ * users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger ToggleWhenActive(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the trigger becomes active, and be cancelled
+ * when it again becomes active. Transfers command ownership to the button
+ * scheduler, so the user does not have to worry about lifespan - rvalue refs
+ * will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ if (command->IsScheduled()) {
+ command->Cancel();
+ } else {
+ command->Schedule(interruptible);
+ }
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a command to be cancelled when the trigger becomes active. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * and scheduling of the command.
+ *
+ * @param command The command to bind.
+ * @return The trigger, for chained calls.
+ */
+ Trigger CancelWhenActive(Command* command);
+
+ /**
+ * Composes two triggers with logical AND.
+ *
+ * @return A trigger which is active when both component triggers are active.
+ */
+ Trigger operator&&(Trigger rhs) {
+ return Trigger([*this, rhs] { return Get() && rhs.Get(); });
+ }
+
+ /**
+ * Composes two triggers with logical OR.
+ *
+ * @return A trigger which is active when either component trigger is active.
+ */
+ Trigger operator||(Trigger rhs) {
+ return Trigger([*this, rhs] { return Get() || rhs.Get(); });
+ }
+
+ /**
+ * Composes a trigger with logical NOT.
+ *
+ * @return A trigger which is active when the component trigger is inactive,
+ * and vice-versa.
+ */
+ Trigger operator!() {
+ return Trigger([*this] { return !Get(); });
+ }
+
+ private:
+ std::function<bool()> m_isActive;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
new file mode 100644
index 0000000..4f9abf0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/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.wpilibj2;
+
+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/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
new file mode 100644
index 0000000..64354c3
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
@@ -0,0 +1,179 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj2.command.button.InternalButton;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+
+class ButtonTest extends CommandTestBase {
+ @Test
+ void whenPressedTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ InternalButton button = new InternalButton();
+ button.setPressed(false);
+ button.whenPressed(command1);
+ scheduler.run();
+ verify(command1, never()).schedule(true);
+ button.setPressed(true);
+ scheduler.run();
+ scheduler.run();
+ verify(command1).schedule(true);
+ }
+
+ @Test
+ void whenReleasedTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ InternalButton button = new InternalButton();
+ button.setPressed(true);
+ button.whenReleased(command1);
+ scheduler.run();
+ verify(command1, never()).schedule(true);
+ button.setPressed(false);
+ scheduler.run();
+ scheduler.run();
+ verify(command1).schedule(true);
+ }
+
+ @Test
+ void whileHeldTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ InternalButton button = new InternalButton();
+ button.setPressed(false);
+ button.whileHeld(command1);
+ scheduler.run();
+ verify(command1, never()).schedule(true);
+ button.setPressed(true);
+ scheduler.run();
+ scheduler.run();
+ verify(command1, times(2)).schedule(true);
+ button.setPressed(false);
+ scheduler.run();
+ verify(command1).cancel();
+ }
+
+ @Test
+ void whenHeldTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ InternalButton button = new InternalButton();
+ button.setPressed(false);
+ button.whenHeld(command1);
+ scheduler.run();
+ verify(command1, never()).schedule(true);
+ button.setPressed(true);
+ scheduler.run();
+ scheduler.run();
+ verify(command1).schedule(true);
+ button.setPressed(false);
+ scheduler.run();
+ verify(command1).cancel();
+ }
+
+ @Test
+ void toggleWhenPressedTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ InternalButton button = new InternalButton();
+ button.setPressed(false);
+ button.toggleWhenPressed(command1);
+ scheduler.run();
+ verify(command1, never()).schedule(true);
+ button.setPressed(true);
+ scheduler.run();
+ when(command1.isScheduled()).thenReturn(true);
+ scheduler.run();
+ verify(command1).schedule(true);
+ button.setPressed(false);
+ scheduler.run();
+ verify(command1, never()).cancel();
+ button.setPressed(true);
+ scheduler.run();
+ verify(command1).cancel();
+ }
+
+ @Test
+ void cancelWhenPressedTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ InternalButton button = new InternalButton();
+ button.setPressed(false);
+ button.cancelWhenPressed(command1);
+ scheduler.run();
+ verify(command1, never()).cancel();
+ button.setPressed(true);
+ scheduler.run();
+ scheduler.run();
+ verify(command1).cancel();
+ }
+
+ @Test
+ void runnableBindingTest() {
+
+ InternalButton buttonWhenPressed = new InternalButton();
+ InternalButton buttonWhileHeld = new InternalButton();
+ InternalButton buttonWhenReleased = new InternalButton();
+
+ buttonWhenPressed.setPressed(false);
+ buttonWhileHeld.setPressed(true);
+ buttonWhenReleased.setPressed(true);
+
+ Counter counter = new Counter();
+
+ buttonWhenPressed.whenPressed(counter::increment);
+ buttonWhileHeld.whileHeld(counter::increment);
+ buttonWhenReleased.whenReleased(counter::increment);
+
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+
+ scheduler.run();
+ buttonWhenPressed.setPressed(true);
+ buttonWhenReleased.setPressed(false);
+ scheduler.run();
+
+ assertEquals(counter.m_counter, 4);
+ }
+
+ @Test
+ void buttonCompositionTest() {
+ InternalButton button1 = new InternalButton();
+ InternalButton button2 = new InternalButton();
+
+ button1.setPressed(true);
+ button2.setPressed(false);
+
+ assertFalse(button1.and(button2).get());
+ assertTrue(button1.or(button2).get());
+ assertFalse(button1.negate().get());
+ assertTrue(button1.and(button2.negate()).get());
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
new file mode 100644
index 0000000..23f63c7
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class CommandDecoratorTest extends CommandTestBase {
+ @Test
+ void withTimeoutTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Command command1 = new WaitCommand(10);
+
+ Command timeout = command1.withTimeout(2);
+
+ scheduler.schedule(timeout);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(command1));
+ assertTrue(scheduler.isScheduled(timeout));
+
+ Timer.delay(3);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(timeout));
+ }
+
+ @Test
+ void withInterruptTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+
+ Command command = new WaitCommand(10).withInterrupt(condition::getCondition);
+
+ scheduler.schedule(command);
+ scheduler.run();
+ assertTrue(scheduler.isScheduled(command));
+ condition.setCondition(true);
+ scheduler.run();
+ assertFalse(scheduler.isScheduled(command));
+ }
+
+ @Test
+ void beforeStartingTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+ condition.setCondition(false);
+
+ Command command = new InstantCommand();
+
+ scheduler.schedule(command.beforeStarting(() -> condition.setCondition(true)));
+
+ assertTrue(condition.getCondition());
+ }
+
+ @Test
+ void andThenLambdaTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+ condition.setCondition(false);
+
+ Command command = new InstantCommand();
+
+ scheduler.schedule(command.andThen(() -> condition.setCondition(true)));
+
+ assertFalse(condition.getCondition());
+
+ scheduler.run();
+
+ assertTrue(condition.getCondition());
+ }
+
+ @Test
+ void andThenTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+ condition.setCondition(false);
+
+ Command command1 = new InstantCommand();
+ Command command2 = new InstantCommand(() -> condition.setCondition(true));
+
+ scheduler.schedule(command1.andThen(command2));
+
+ assertFalse(condition.getCondition());
+
+ scheduler.run();
+
+ assertTrue(condition.getCondition());
+ }
+
+ @Test
+ void deadlineWithTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+ condition.setCondition(false);
+
+ Command dictator = new WaitUntilCommand(condition::getCondition);
+ Command endsBefore = new InstantCommand();
+ Command endsAfter = new WaitUntilCommand(() -> false);
+
+ Command group = dictator.deadlineWith(endsBefore, endsAfter);
+
+ scheduler.schedule(group);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(group));
+
+ condition.setCondition(true);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void alongWithTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+ condition.setCondition(false);
+
+ Command command1 = new WaitUntilCommand(condition::getCondition);
+ Command command2 = new InstantCommand();
+
+ Command group = command1.alongWith(command2);
+
+ scheduler.schedule(group);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(group));
+
+ condition.setCondition(true);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void raceWithTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Command command1 = new WaitUntilCommand(() -> false);
+ Command command2 = new InstantCommand();
+
+ Command group = command1.raceWith(command2);
+
+ scheduler.schedule(group);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void perpetuallyTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Command command = new InstantCommand();
+
+ Command perpetual = command.perpetually();
+
+ scheduler.schedule(perpetual);
+ scheduler.run();
+ scheduler.run();
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(perpetual));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
new file mode 100644
index 0000000..e453d94
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+class CommandGroupErrorTest extends CommandTestBase {
+ @Test
+ void commandInMultipleGroupsTest() {
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ @SuppressWarnings("PMD.UnusedLocalVariable")
+ Command group = new ParallelCommandGroup(command1, command2);
+ assertThrows(IllegalArgumentException.class,
+ () -> new ParallelCommandGroup(command1, command2));
+ }
+
+ @Test
+ void commandInGroupExternallyScheduledTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ @SuppressWarnings("PMD.UnusedLocalVariable")
+ Command group = new ParallelCommandGroup(command1, command2);
+
+ assertThrows(IllegalArgumentException.class,
+ () -> scheduler.schedule(command1));
+ }
+
+ @Test
+ void redecoratedCommandErrorTest() {
+ Command command = new InstantCommand();
+
+ assertDoesNotThrow(() -> command.withTimeout(10).withInterrupt(() -> false));
+ assertThrows(IllegalArgumentException.class, () -> command.withTimeout(10));
+ CommandGroupBase.clearGroupedCommand(command);
+ assertDoesNotThrow(() -> command.withTimeout(10));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
new file mode 100644
index 0000000..10d3ee0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+@SuppressWarnings("PMD.TooManyMethods")
+class CommandRequirementsTest extends CommandTestBase {
+ @Test
+ void requirementInterruptTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Subsystem requirement = new TestSubsystem();
+
+ MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+ Command interrupted = interruptedHolder.getMock();
+ MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+ Command interrupter = interrupterHolder.getMock();
+
+ scheduler.schedule(interrupted);
+ scheduler.run();
+ scheduler.schedule(interrupter);
+ scheduler.run();
+
+ verify(interrupted).initialize();
+ verify(interrupted).execute();
+ verify(interrupted).end(true);
+
+ verify(interrupter).initialize();
+ verify(interrupter).execute();
+
+ assertFalse(scheduler.isScheduled(interrupted));
+ assertTrue(scheduler.isScheduled(interrupter));
+ }
+
+ @Test
+ void requirementUninterruptibleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Subsystem requirement = new TestSubsystem();
+
+ MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+ Command notInterrupted = interruptedHolder.getMock();
+ MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+ Command interrupter = interrupterHolder.getMock();
+
+ scheduler.schedule(false, notInterrupted);
+ scheduler.schedule(interrupter);
+
+ assertTrue(scheduler.isScheduled(notInterrupted));
+ assertFalse(scheduler.isScheduled(interrupter));
+ }
+
+ @Test
+ void defaultCommandRequirementErrorTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Subsystem system = new TestSubsystem();
+
+ Command missingRequirement = new WaitUntilCommand(() -> false);
+ Command ends = new InstantCommand(() -> {
+ }, system);
+
+ assertThrows(IllegalArgumentException.class,
+ () -> scheduler.setDefaultCommand(system, missingRequirement));
+ assertThrows(IllegalArgumentException.class,
+ () -> scheduler.setDefaultCommand(system, ends));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
new file mode 100644
index 0000000..aee30db
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class CommandScheduleTest extends CommandTestBase {
+ @Test
+ void instantScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder holder = new MockCommandHolder(true);
+ holder.setFinished(true);
+ Command mockCommand = holder.getMock();
+
+ scheduler.schedule(mockCommand);
+ assertTrue(scheduler.isScheduled(mockCommand));
+ verify(mockCommand).initialize();
+
+ scheduler.run();
+
+ verify(mockCommand).execute();
+ verify(mockCommand).end(false);
+
+ assertFalse(scheduler.isScheduled(mockCommand));
+ }
+
+ @Test
+ void singleIterationScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder holder = new MockCommandHolder(true);
+ Command mockCommand = holder.getMock();
+
+ scheduler.schedule(mockCommand);
+
+ assertTrue(scheduler.isScheduled(mockCommand));
+
+ scheduler.run();
+ holder.setFinished(true);
+ scheduler.run();
+
+ verify(mockCommand).initialize();
+ verify(mockCommand, times(2)).execute();
+ verify(mockCommand).end(false);
+
+ assertFalse(scheduler.isScheduled(mockCommand));
+ }
+
+ @Test
+ void multiScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+
+ scheduler.schedule(true, command1, command2, command3);
+ assertTrue(scheduler.isScheduled(command1, command2, command3));
+ scheduler.run();
+ assertTrue(scheduler.isScheduled(command1, command2, command3));
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+ assertTrue(scheduler.isScheduled(command2, command3));
+ assertFalse(scheduler.isScheduled(command1));
+
+ command2Holder.setFinished(true);
+ scheduler.run();
+ assertTrue(scheduler.isScheduled(command3));
+ assertFalse(scheduler.isScheduled(command1, command2));
+
+ command3Holder.setFinished(true);
+ scheduler.run();
+ assertFalse(scheduler.isScheduled(command1, command2, command3));
+ }
+
+ @Test
+ void schedulerCancelTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder holder = new MockCommandHolder(true);
+ Command mockCommand = holder.getMock();
+
+ scheduler.schedule(mockCommand);
+
+ scheduler.run();
+ scheduler.cancel(mockCommand);
+ scheduler.run();
+
+ verify(mockCommand).execute();
+ verify(mockCommand).end(true);
+ verify(mockCommand, never()).end(false);
+
+ assertFalse(scheduler.isScheduled(mockCommand));
+ }
+
+ @Test
+ void notScheduledCancelTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder holder = new MockCommandHolder(true);
+ Command mockCommand = holder.getMock();
+
+ assertDoesNotThrow(() -> scheduler.cancel(mockCommand));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
new file mode 100644
index 0000000..61aa1a0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+
+import org.junit.jupiter.api.BeforeEach;
+
+import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.DriverStation;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+/**
+ * Basic setup for all {@link Command tests}."
+ */
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+abstract class CommandTestBase {
+ @BeforeEach
+ void commandSetup() {
+ CommandScheduler.getInstance().cancelAll();
+ CommandScheduler.getInstance().enable();
+ CommandScheduler.getInstance().clearButtons();
+ CommandGroupBase.clearGroupedCommands();
+
+ setDSEnabled(true);
+ }
+
+ void setDSEnabled(boolean enabled) {
+ DriverStationSim sim = new DriverStationSim();
+ sim.setDsAttached(true);
+
+ sim.setEnabled(enabled);
+ sim.notifyNewData();
+ DriverStation.getInstance().isNewControlData();
+ while (DriverStation.getInstance().isEnabled() != enabled) {
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException exception) {
+ exception.printStackTrace();
+ }
+ }
+ }
+
+ class TestSubsystem extends SubsystemBase {
+ }
+
+ protected class MockCommandHolder {
+ private final Command m_mockCommand = mock(Command.class);
+
+ MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
+ when(m_mockCommand.getRequirements()).thenReturn(Set.of(requirements));
+ when(m_mockCommand.isFinished()).thenReturn(false);
+ when(m_mockCommand.runsWhenDisabled()).thenReturn(runWhenDisabled);
+ }
+
+ Command getMock() {
+ return m_mockCommand;
+ }
+
+ void setFinished(boolean finished) {
+ when(m_mockCommand.isFinished()).thenReturn(finished);
+ }
+
+ }
+
+ protected class Counter {
+ int m_counter;
+
+ void increment() {
+ m_counter++;
+ }
+ }
+
+ protected class ConditionHolder {
+ private boolean m_condition;
+
+ void setCondition(boolean condition) {
+ m_condition = condition;
+ }
+
+ boolean getCondition() {
+ return m_condition;
+ }
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
new file mode 100644
index 0000000..0740565
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class ConditionalCommandTest extends CommandTestBase {
+ @Test
+ void conditionalCommandTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ command1Holder.setFinished(true);
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+
+ scheduler.schedule(conditionalCommand);
+ scheduler.run();
+
+ verify(command1).initialize();
+ verify(command1).execute();
+ verify(command1).end(false);
+
+ verify(command2, never()).initialize();
+ verify(command2, never()).execute();
+ verify(command2, never()).end(false);
+ }
+
+ @Test
+ void conditionalCommandRequirementTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+ Command command2 = command2Holder.getMock();
+
+ ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+
+ scheduler.schedule(conditionalCommand);
+ scheduler.schedule(new InstantCommand(() -> {
+ }, system3));
+
+ assertFalse(scheduler.isScheduled(conditionalCommand));
+
+ verify(command1).end(true);
+ verify(command2, never()).end(true);
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
new file mode 100644
index 0000000..033dcc5
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+class DefaultCommandTest extends CommandTestBase {
+ @Test
+ void defaultCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+ MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+ Command defaultCommand = defaultHolder.getMock();
+
+ scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(defaultCommand));
+ }
+
+ @Test
+ void defaultCommandInterruptResumeTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+ MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+ Command defaultCommand = defaultHolder.getMock();
+ MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand);
+ Command interrupter = interrupterHolder.getMock();
+
+ scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+ scheduler.run();
+ scheduler.schedule(interrupter);
+
+ assertFalse(scheduler.isScheduled(defaultCommand));
+ assertTrue(scheduler.isScheduled(interrupter));
+
+ scheduler.cancel(interrupter);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(defaultCommand));
+ assertFalse(scheduler.isScheduled(interrupter));
+ }
+
+ @Test
+ void defaultCommandDisableResumeTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ TestSubsystem hasDefaultCommand = new TestSubsystem();
+
+ MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand);
+ Command defaultCommand = defaultHolder.getMock();
+
+ scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(defaultCommand));
+
+ setDSEnabled(false);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(defaultCommand));
+
+ setDSEnabled(true);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(defaultCommand));
+
+ verify(defaultCommand).end(true);
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
new file mode 100644
index 0000000..23f508a
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class FunctionalCommandTest extends CommandTestBase {
+ @Test
+ void functionalCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder cond1 = new ConditionHolder();
+ ConditionHolder cond2 = new ConditionHolder();
+ ConditionHolder cond3 = new ConditionHolder();
+ ConditionHolder cond4 = new ConditionHolder();
+
+ FunctionalCommand command =
+ new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true),
+ interrupted -> cond3.setCondition(true), cond4::getCondition);
+
+ scheduler.schedule(command);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(command));
+
+ cond4.setCondition(true);
+
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(command));
+ assertTrue(cond1.getCondition());
+ assertTrue(cond2.getCondition());
+ assertTrue(cond3.getCondition());
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
new file mode 100644
index 0000000..a109ed6
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class InstantCommandTest extends CommandTestBase {
+ @Test
+ void instantCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder cond = new ConditionHolder();
+
+ InstantCommand command = new InstantCommand(() -> cond.setCondition(true));
+
+ scheduler.schedule(command);
+ scheduler.run();
+
+ assertTrue(cond.getCondition());
+ assertFalse(scheduler.isScheduled(command));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
new file mode 100644
index 0000000..2ac2129
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumControllerCommandTest {
+ private final Timer m_timer = new Timer();
+ private Rotation2d m_angle = new Rotation2d(0);
+
+ private double m_frontLeftSpeed;
+ private double m_rearLeftSpeed;
+ private double m_frontRightSpeed;
+ private double m_rearRightSpeed;
+
+ private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+ new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+ private static final double kxTolerance = 1 / 12.0;
+ private static final double kyTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = 1 / 12.0;
+
+ private static final double kWheelBase = 0.5;
+ private static final double kTrackWidth = 0.5;
+
+ private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+ new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+ public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+ this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
+ this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond;
+ this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond;
+ this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
+ }
+
+ public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+ return new MecanumDriveWheelSpeeds(m_frontLeftSpeed,
+ m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
+ }
+
+ public Pose2d getRobotPose() {
+ m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds());
+ return m_odometry.getPoseMeters();
+ }
+
+ @Test
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ void testReachesReference() {
+ final var subsystem = new Subsystem() {};
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+ waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+ final var command = new MecanumControllerCommand(trajectory,
+ this::getRobotPose,
+ m_kinematics,
+ new PIDController(0.6, 0, 0),
+ new PIDController(0.6, 0, 0),
+ m_rotController,
+ 8.8,
+ this::setWheelSpeeds,
+ subsystem);
+
+ m_timer.reset();
+ m_timer.start();
+
+ command.initialize();
+ while (!command.isFinished()) {
+ command.execute();
+ m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+ }
+ m_timer.stop();
+ command.end(true);
+
+ assertAll(
+ () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+ getRobotPose().getTranslation().getX(), kxTolerance),
+ () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+ getRobotPose().getTranslation().getY(), kyTolerance),
+ () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+ getRobotPose().getRotation().getRadians(), kAngularTolerance)
+ );
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
new file mode 100644
index 0000000..fb06844
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.condition.DisabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@DisabledOnOs(OS.MAC)
+class NotifierCommandTest extends CommandTestBase {
+ @Test
+ void notifierCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Counter counter = new Counter();
+
+ NotifierCommand command = new NotifierCommand(counter::increment, 0.01);
+
+ scheduler.schedule(command);
+ Timer.delay(0.25);
+ scheduler.cancel(command);
+
+ assertTrue(counter.m_counter > 10, "Should have hit at least 10 triggers");
+ assertTrue(counter.m_counter < 100, "Shouldn't hit more then 100 triggers");
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
new file mode 100644
index 0000000..d03b4aa
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class ParallelCommandGroupTest extends CommandTestBase {
+ @Test
+ void parallelGroupScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new ParallelCommandGroup(command1, command2);
+
+ scheduler.schedule(group);
+
+ verify(command1).initialize();
+ verify(command2).initialize();
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+ command2Holder.setFinished(true);
+ scheduler.run();
+
+ verify(command1).execute();
+ verify(command1).end(false);
+ verify(command2, times(2)).execute();
+ verify(command2).end(false);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void parallelGroupInterruptTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new ParallelCommandGroup(command1, command2);
+
+ scheduler.schedule(group);
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+ scheduler.run();
+ scheduler.cancel(group);
+
+ verify(command1).execute();
+ verify(command1).end(false);
+ verify(command1, never()).end(true);
+
+ verify(command2, times(2)).execute();
+ verify(command2, never()).end(false);
+ verify(command2).end(true);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void notScheduledCancelTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new ParallelCommandGroup(command1, command2);
+
+ assertDoesNotThrow(() -> scheduler.cancel(group));
+ }
+
+ @Test
+ void parallelGroupRequirementTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+ Subsystem system4 = new TestSubsystem();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+ Command command3 = command3Holder.getMock();
+
+ Command group = new ParallelCommandGroup(command1, command2);
+
+ scheduler.schedule(group);
+ scheduler.schedule(command3);
+
+ assertFalse(scheduler.isScheduled(group));
+ assertTrue(scheduler.isScheduled(command3));
+ }
+
+ @Test
+ void parallelGroupRequirementErrorTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+ Command command2 = command2Holder.getMock();
+
+ assertThrows(IllegalArgumentException.class,
+ () -> new ParallelCommandGroup(command1, command2));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
new file mode 100644
index 0000000..c7e3769
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.internal.verification.VerificationModeFactory.times;
+
+class ParallelDeadlineGroupTest extends CommandTestBase {
+ @Test
+ void parallelDeadlineScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ command2Holder.setFinished(true);
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+
+ Command group = new ParallelDeadlineGroup(command1, command2, command3);
+
+ scheduler.schedule(group);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(group));
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(group));
+
+ verify(command2).initialize();
+ verify(command2).execute();
+ verify(command2).end(false);
+ verify(command2, never()).end(true);
+
+ verify(command1).initialize();
+ verify(command1, times(2)).execute();
+ verify(command1).end(false);
+ verify(command1, never()).end(true);
+
+ verify(command3).initialize();
+ verify(command3, times(2)).execute();
+ verify(command3, never()).end(false);
+ verify(command3).end(true);
+ }
+
+ @Test
+ void parallelDeadlineInterruptTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ command2Holder.setFinished(true);
+
+ Command group = new ParallelDeadlineGroup(command1, command2);
+
+ scheduler.schedule(group);
+
+ scheduler.run();
+ scheduler.run();
+ scheduler.cancel(group);
+
+ verify(command1, times(2)).execute();
+ verify(command1, never()).end(false);
+ verify(command1).end(true);
+
+ verify(command2).execute();
+ verify(command2).end(false);
+ verify(command2, never()).end(true);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+
+ @Test
+ void parallelDeadlineRequirementTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+ Subsystem system4 = new TestSubsystem();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+ Command command3 = command3Holder.getMock();
+
+ Command group = new ParallelDeadlineGroup(command1, command2);
+
+ scheduler.schedule(group);
+ scheduler.schedule(command3);
+
+ assertFalse(scheduler.isScheduled(group));
+ assertTrue(scheduler.isScheduled(command3));
+ }
+
+ @Test
+ void parallelDeadlineRequirementErrorTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+ Command command2 = command2Holder.getMock();
+
+ assertThrows(IllegalArgumentException.class,
+ () -> new ParallelDeadlineGroup(command1, command2));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
new file mode 100644
index 0000000..8a4781f
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+class ParallelRaceGroupTest extends CommandTestBase {
+ @Test
+ void parallelRaceScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new ParallelRaceGroup(command1, command2);
+
+ scheduler.schedule(group);
+
+ verify(command1).initialize();
+ verify(command2).initialize();
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+ command2Holder.setFinished(true);
+ scheduler.run();
+
+ verify(command1).execute();
+ verify(command1).end(false);
+ verify(command2).execute();
+ verify(command2).end(true);
+ verify(command2, never()).end(false);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void parallelRaceInterruptTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new ParallelRaceGroup(command1, command2);
+
+ scheduler.schedule(group);
+
+ scheduler.run();
+ scheduler.run();
+ scheduler.cancel(group);
+
+ verify(command1, times(2)).execute();
+ verify(command1, never()).end(false);
+ verify(command1).end(true);
+
+ verify(command2, times(2)).execute();
+ verify(command2, never()).end(false);
+ verify(command2).end(true);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void notScheduledCancelTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new ParallelRaceGroup(command1, command2);
+
+ assertDoesNotThrow(() -> scheduler.cancel(group));
+ }
+
+
+ @Test
+ void parallelRaceRequirementTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+ Subsystem system4 = new TestSubsystem();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+ Command command3 = command3Holder.getMock();
+
+ Command group = new ParallelRaceGroup(command1, command2);
+
+ scheduler.schedule(group);
+ scheduler.schedule(command3);
+
+ assertFalse(scheduler.isScheduled(group));
+ assertTrue(scheduler.isScheduled(command3));
+ }
+
+ @Test
+ void parallelRaceRequirementErrorTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+ Command command2 = command2Holder.getMock();
+
+ assertThrows(IllegalArgumentException.class, () -> new ParallelRaceGroup(command1, command2));
+ }
+
+ @Test
+ void parallelRaceOnlyCallsEndOnceTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system2);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder perpetualCommandHolder = new MockCommandHolder(true);
+ Command command3 = new PerpetualCommand(perpetualCommandHolder.getMock());
+
+ Command group1 = new SequentialCommandGroup(command1, command2);
+ assertNotNull(group1);
+ assertNotNull(command3);
+ Command group2 = new ParallelRaceGroup(group1, command3);
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ scheduler.schedule(group2);
+ scheduler.run();
+ command1Holder.setFinished(true);
+ scheduler.run();
+ command2Holder.setFinished(true);
+ // at this point the sequential group should be done
+ assertDoesNotThrow(() -> scheduler.run());
+
+ }
+
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
new file mode 100644
index 0000000..baf037f
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PerpetualCommandTest extends CommandTestBase {
+ @Test
+ void perpetualCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ PerpetualCommand command = new PerpetualCommand(new InstantCommand());
+
+ scheduler.schedule(command);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(command));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
new file mode 100644
index 0000000..7484396
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.io.ByteArrayOutputStream;
+import java.io.PrintStream;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+class PrintCommandTest extends CommandTestBase {
+ @Test
+ void printCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ final PrintStream originalOut = System.out;
+ ByteArrayOutputStream testOut = new ByteArrayOutputStream();
+ System.setOut(new PrintStream(testOut));
+
+ PrintCommand command = new PrintCommand("Test!");
+
+ scheduler.schedule(command);
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(command));
+ assertEquals(testOut.toString(), "Test!" + System.lineSeparator());
+
+ System.setOut(originalOut);
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
new file mode 100644
index 0000000..b566aae
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+class ProxyScheduleCommandTest extends CommandTestBase {
+ @Test
+ void proxyScheduleCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+
+ ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command1);
+
+ scheduler.schedule(scheduleCommand);
+
+ verify(command1).schedule();
+ }
+
+ @Test
+ void proxyScheduleCommandEndTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+
+ ConditionHolder cond = new ConditionHolder();
+
+ WaitUntilCommand command = new WaitUntilCommand(cond::getCondition);
+
+ ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command);
+
+ scheduler.schedule(scheduleCommand);
+
+ scheduler.run();
+ assertTrue(scheduler.isScheduled(scheduleCommand));
+
+ cond.setCondition(true);
+ scheduler.run();
+ scheduler.run();
+ assertFalse(scheduler.isScheduled(scheduleCommand));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
new file mode 100644
index 0000000..e862216
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+
+import org.junit.jupiter.api.Test;
+
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.parallel;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class RobotDisabledCommandTest extends CommandTestBase {
+ @Test
+ void robotDisabledCommandCancelTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder holder = new MockCommandHolder(false);
+ Command mockCommand = holder.getMock();
+
+ scheduler.schedule(mockCommand);
+
+ assertTrue(scheduler.isScheduled(mockCommand));
+
+ setDSEnabled(false);
+
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(mockCommand));
+
+ setDSEnabled(true);
+ }
+
+ @Test
+ void runWhenDisabledTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder holder = new MockCommandHolder(true);
+ Command mockCommand = holder.getMock();
+
+ scheduler.schedule(mockCommand);
+
+ assertTrue(scheduler.isScheduled(mockCommand));
+
+ setDSEnabled(false);
+
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(mockCommand));
+ }
+
+ @Test
+ void sequentialGroupRunWhenDisabledTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+ MockCommandHolder command4Holder = new MockCommandHolder(false);
+ Command command4 = command4Holder.getMock();
+
+ Command runWhenDisabled = new SequentialCommandGroup(command1, command2);
+ Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4);
+
+ scheduler.schedule(runWhenDisabled);
+ scheduler.schedule(dontRunWhenDisabled);
+
+ setDSEnabled(false);
+
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(runWhenDisabled));
+ assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+ }
+
+ @Test
+ void parallelGroupRunWhenDisabledTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+ MockCommandHolder command4Holder = new MockCommandHolder(false);
+ Command command4 = command4Holder.getMock();
+
+ Command runWhenDisabled = new ParallelCommandGroup(command1, command2);
+ Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4);
+
+ scheduler.schedule(runWhenDisabled);
+ scheduler.schedule(dontRunWhenDisabled);
+
+ setDSEnabled(false);
+
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(runWhenDisabled));
+ assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+ }
+
+ @Test
+ void conditionalRunWhenDisabledTest() {
+ setDSEnabled(false);
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+ MockCommandHolder command4Holder = new MockCommandHolder(false);
+ Command command4 = command4Holder.getMock();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+ Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+ scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+ assertTrue(scheduler.isScheduled(runWhenDisabled));
+ assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+ }
+
+ @Test
+ void selectRunWhenDisabledTest() {
+ setDSEnabled(false);
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+ MockCommandHolder command4Holder = new MockCommandHolder(false);
+ Command command4 = command4Holder.getMock();
+
+ Command runWhenDisabled = new SelectCommand(Map.of(1, command1, 2, command2), () -> 1);
+ Command dontRunWhenDisabled = new SelectCommand(Map.of(1, command3, 2, command4), () -> 1);
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+ assertTrue(scheduler.isScheduled(runWhenDisabled));
+ assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+ }
+
+ @Test
+ void parallelConditionalRunWhenDisabledTest() {
+ setDSEnabled(false);
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+ MockCommandHolder command4Holder = new MockCommandHolder(false);
+ Command command4 = command4Holder.getMock();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+ Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+ Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled);
+
+ scheduler.schedule(parallel);
+
+ assertFalse(scheduler.isScheduled(runWhenDisabled));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
new file mode 100644
index 0000000..3ff8c3d
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class RunCommandTest extends CommandTestBase {
+ @Test
+ void runCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Counter counter = new Counter();
+
+ RunCommand command = new RunCommand(counter::increment);
+
+ scheduler.schedule(command);
+ scheduler.run();
+ scheduler.run();
+ scheduler.run();
+
+ assertEquals(3, counter.m_counter);
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
new file mode 100644
index 0000000..9041627
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.mockito.Mockito.verify;
+
+class ScheduleCommandTest extends CommandTestBase {
+ @Test
+ void scheduleCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2);
+
+ scheduler.schedule(scheduleCommand);
+
+ verify(command1).schedule();
+ verify(command2).schedule();
+ }
+
+ @Test
+ void scheduleCommandDuringRunTest() {
+ CommandScheduler scheduler = CommandScheduler.getInstance();
+
+ InstantCommand toSchedule = new InstantCommand();
+ ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule);
+ SequentialCommandGroup group =
+ new SequentialCommandGroup(new InstantCommand(), scheduleCommand);
+
+ scheduler.schedule(group);
+ scheduler.schedule(new InstantCommand().perpetually());
+ scheduler.run();
+ assertDoesNotThrow(scheduler::run);
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
new file mode 100644
index 0000000..1f110b6
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SchedulerTest extends CommandTestBase {
+ @Test
+ void schedulerLambdaTestNoInterrupt() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Counter counter = new Counter();
+
+ scheduler.onCommandInitialize(command -> counter.increment());
+ scheduler.onCommandExecute(command -> counter.increment());
+ scheduler.onCommandFinish(command -> counter.increment());
+
+ scheduler.schedule(new InstantCommand());
+ scheduler.run();
+
+ assertEquals(counter.m_counter, 3);
+ }
+
+ @Test
+ void schedulerInterruptLambdaTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Counter counter = new Counter();
+
+ scheduler.onCommandInterrupt(command -> counter.increment());
+
+ Command command = new WaitCommand(10);
+
+ scheduler.schedule(command);
+ scheduler.cancel(command);
+
+ assertEquals(counter.m_counter, 1);
+ }
+
+ @Test
+ void unregisterSubsystemTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ Subsystem system = new TestSubsystem();
+
+ scheduler.registerSubsystem(system);
+ assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
new file mode 100644
index 0000000..df52855
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Map;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class SelectCommandTest extends CommandTestBase {
+ @Test
+ void selectCommandTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ command1Holder.setFinished(true);
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+
+ SelectCommand selectCommand =
+ new SelectCommand(Map.ofEntries(
+ Map.entry("one", command1),
+ Map.entry("two", command2),
+ Map.entry("three", command3)),
+ () -> "one");
+
+ scheduler.schedule(selectCommand);
+ scheduler.run();
+
+ verify(command1).initialize();
+ verify(command1).execute();
+ verify(command1).end(false);
+
+ verify(command2, never()).initialize();
+ verify(command2, never()).execute();
+ verify(command2, never()).end(false);
+
+ verify(command3, never()).initialize();
+ verify(command3, never()).execute();
+ verify(command3, never()).end(false);
+ }
+
+ @Test
+ void selectCommandInvalidKeyTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ command1Holder.setFinished(true);
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+
+ SelectCommand selectCommand =
+ new SelectCommand(Map.ofEntries(
+ Map.entry("one", command1),
+ Map.entry("two", command2),
+ Map.entry("three", command3)),
+ () -> "four");
+
+ assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
+ }
+
+
+ @Test
+ void selectCommandRequirementTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+ Subsystem system4 = new TestSubsystem();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+ Command command3 = command3Holder.getMock();
+
+ SelectCommand selectCommand = new SelectCommand(
+ Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2),
+ Map.entry("three", command3)), () -> "one");
+
+ scheduler.schedule(selectCommand);
+ scheduler.schedule(new InstantCommand(() -> {
+ }, system3));
+
+ assertFalse(scheduler.isScheduled(selectCommand));
+
+ verify(command1).end(true);
+ verify(command2, never()).end(true);
+ verify(command3, never()).end(true);
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
new file mode 100644
index 0000000..8582140
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class SequentialCommandGroupTest extends CommandTestBase {
+ @Test
+ void sequentialGroupScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new SequentialCommandGroup(command1, command2);
+
+ scheduler.schedule(group);
+
+ verify(command1).initialize();
+ verify(command2, never()).initialize();
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+
+ verify(command1).execute();
+ verify(command1).end(false);
+ verify(command2).initialize();
+ verify(command2, never()).execute();
+ verify(command2, never()).end(false);
+
+ command2Holder.setFinished(true);
+ scheduler.run();
+
+ verify(command1).execute();
+ verify(command1).end(false);
+ verify(command2).execute();
+ verify(command2).end(false);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void sequentialGroupInterruptTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true);
+ Command command3 = command3Holder.getMock();
+
+ Command group = new SequentialCommandGroup(command1, command2, command3);
+
+ scheduler.schedule(group);
+
+ command1Holder.setFinished(true);
+ scheduler.run();
+ scheduler.cancel(group);
+ scheduler.run();
+
+ verify(command1).execute();
+ verify(command1, never()).end(true);
+ verify(command1).end(false);
+ verify(command2, never()).execute();
+ verify(command2).end(true);
+ verify(command2, never()).end(false);
+ verify(command3, never()).initialize();
+ verify(command3, never()).execute();
+ verify(command3, never()).end(true);
+ verify(command3, never()).end(false);
+
+ assertFalse(scheduler.isScheduled(group));
+ }
+
+ @Test
+ void notScheduledCancelTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true);
+ Command command2 = command2Holder.getMock();
+
+ Command group = new SequentialCommandGroup(command1, command2);
+
+ assertDoesNotThrow(() -> scheduler.cancel(group));
+ }
+
+
+ @Test
+ void sequentialGroupRequirementTest() {
+ Subsystem system1 = new TestSubsystem();
+ Subsystem system2 = new TestSubsystem();
+ Subsystem system3 = new TestSubsystem();
+ Subsystem system4 = new TestSubsystem();
+
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+ Command command1 = command1Holder.getMock();
+ MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+ Command command2 = command2Holder.getMock();
+ MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+ Command command3 = command3Holder.getMock();
+
+ Command group = new SequentialCommandGroup(command1, command2);
+
+ scheduler.schedule(group);
+ scheduler.schedule(command3);
+
+ assertFalse(scheduler.isScheduled(group));
+ assertTrue(scheduler.isScheduled(command3));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
new file mode 100644
index 0000000..93921e0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class StartEndCommandTest extends CommandTestBase {
+ @Test
+ void startEndCommandScheduleTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder cond1 = new ConditionHolder();
+ ConditionHolder cond2 = new ConditionHolder();
+
+ StartEndCommand command =
+ new StartEndCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true));
+
+ scheduler.schedule(command);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(command));
+
+ scheduler.cancel(command);
+
+ assertFalse(scheduler.isScheduled(command));
+ assertTrue(cond1.getCondition());
+ assertTrue(cond2.getCondition());
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
new file mode 100644
index 0000000..5209199
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveControllerCommandTest {
+ private final Timer m_timer = new Timer();
+ private Rotation2d m_angle = new Rotation2d(0);
+
+ private SwerveModuleState[] m_moduleStates = new SwerveModuleState[]{
+ new SwerveModuleState(0, new Rotation2d(0)),
+ new SwerveModuleState(0, new Rotation2d(0)),
+ new SwerveModuleState(0, new Rotation2d(0)),
+ new SwerveModuleState(0, new Rotation2d(0))};
+
+ private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+ new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+ private static final double kxTolerance = 1 / 12.0;
+ private static final double kyTolerance = 1 / 12.0;
+ private static final double kAngularTolerance = 1 / 12.0;
+
+ private static final double kWheelBase = 0.5;
+ private static final double kTrackWidth = 0.5;
+
+ private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+ new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+ new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+ private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+ new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+ @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+ public void setModuleStates(SwerveModuleState[] moduleStates) {
+ this.m_moduleStates = moduleStates;
+ }
+
+ public Pose2d getRobotPose() {
+ m_odometry.updateWithTime(m_timer.get(), m_angle, m_moduleStates);
+ return m_odometry.getPoseMeters();
+ }
+
+ @Test
+ @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+ void testReachesReference() {
+ final var subsystem = new Subsystem() {};
+
+ final var waypoints = new ArrayList<Pose2d>();
+ waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+ waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+ var config = new TrajectoryConfig(8.8, 0.1);
+ final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+ final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+ final var command = new SwerveControllerCommand(trajectory,
+ this::getRobotPose,
+ m_kinematics,
+ new PIDController(0.6, 0, 0),
+ new PIDController(0.6, 0, 0),
+ m_rotController,
+ this::setModuleStates,
+ subsystem);
+
+ m_timer.reset();
+ m_timer.start();
+
+ command.initialize();
+ while (!command.isFinished()) {
+ command.execute();
+ m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+ }
+ m_timer.stop();
+ command.end(true);
+
+ assertAll(
+ () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+ getRobotPose().getTranslation().getX(), kxTolerance),
+ () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+ getRobotPose().getTranslation().getY(), kyTolerance),
+ () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+ getRobotPose().getRotation().getRadians(), kAngularTolerance)
+ );
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
new file mode 100644
index 0000000..5a522af
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.ArgumentMatchers.anyDouble;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+class WaitCommandTest extends CommandTestBase {
+ @Test
+ void waitCommandTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ WaitCommand waitCommand = new WaitCommand(2);
+
+ scheduler.schedule(waitCommand);
+ scheduler.run();
+ Timer.delay(1);
+ scheduler.run();
+
+ assertTrue(scheduler.isScheduled(waitCommand));
+
+ Timer.delay(2);
+
+ scheduler.run();
+
+ assertFalse(scheduler.isScheduled(waitCommand));
+ }
+
+ @Test
+ void withTimeoutTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ MockCommandHolder command1Holder = new MockCommandHolder(true);
+ Command command1 = command1Holder.getMock();
+ when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+
+ Command timeout = command1.withTimeout(2);
+
+ scheduler.schedule(timeout);
+ scheduler.run();
+
+ verify(command1).initialize();
+ verify(command1).execute();
+ assertFalse(scheduler.isScheduled(command1));
+ assertTrue(scheduler.isScheduled(timeout));
+
+ Timer.delay(3);
+ scheduler.run();
+
+ verify(command1).end(true);
+ verify(command1, never()).end(false);
+ assertFalse(scheduler.isScheduled(timeout));
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
new file mode 100644
index 0000000..a99a18c
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class WaitUntilCommandTest extends CommandTestBase {
+ @Test
+ void waitUntilTest() {
+ CommandScheduler scheduler = new CommandScheduler();
+
+ ConditionHolder condition = new ConditionHolder();
+
+ Command command = new WaitUntilCommand(condition::getCondition);
+
+ scheduler.schedule(command);
+ scheduler.run();
+ assertTrue(scheduler.isScheduled(command));
+ condition.setCondition(true);
+ scheduler.run();
+ assertFalse(scheduler.isScheduled(command));
+ }
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
new file mode 100644
index 0000000..829f0d2
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class ButtonTest : public CommandTestBase {};
+
+TEST_F(ButtonTest, WhenPressedTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ Trigger([&pressed] { return pressed; }).WhenActive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenReleasedTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = true;
+ Trigger([&pressed] { return pressed; }).WhenInactive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhileHeldTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ finished = false;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenHeldTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ finished = false;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+ pressed = true;
+ scheduler.Run();
+ pressed = false;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, ToggleWhenPressedTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ pressed = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, AndTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed1 = false;
+ bool pressed2 = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ (Trigger([&pressed1] { return pressed1; }) &&
+ Trigger([&pressed2] { return pressed2; }))
+ .WhenActive(&command);
+ pressed1 = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed2 = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, OrTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed1 = false;
+ bool pressed2 = false;
+ WaitUntilCommand command1([&finished] { return finished; });
+ WaitUntilCommand command2([&finished] { return finished; });
+
+ (Trigger([&pressed1] { return pressed1; }) ||
+ Trigger([&pressed2] { return pressed2; }))
+ .WhenActive(&command1);
+ pressed1 = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+ pressed1 = false;
+
+ (Trigger([&pressed1] { return pressed1; }) ||
+ Trigger([&pressed2] { return pressed2; }))
+ .WhenActive(&command2);
+ pressed2 = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(ButtonTest, NegateTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = true;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, RValueButtonTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ int counter = 0;
+ bool pressed = false;
+
+ RunCommand command([&counter] { counter++; }, {});
+
+ Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
+ scheduler.Run();
+ EXPECT_EQ(counter, 0);
+ pressed = true;
+ scheduler.Run();
+ EXPECT_EQ(counter, 1);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
new file mode 100644
index 0000000..5e876c4
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandDecoratorTest : public CommandTestBase {};
+
+TEST_F(CommandDecoratorTest, WithTimeoutTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ auto command = RunCommand([] {}, {}).WithTimeout(100_ms);
+
+ scheduler.Schedule(&command);
+
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(150));
+
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, WithInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ auto command =
+ RunCommand([] {}, {}).WithInterrupt([&finished] { return finished; });
+
+ scheduler.Schedule(&command);
+
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+ finished = true;
+
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, BeforeStartingTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ auto command = InstantCommand([] {}, {}).BeforeStarting(
+ [&finished] { finished = true; });
+
+ scheduler.Schedule(&command);
+
+ EXPECT_TRUE(finished);
+
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, AndThenTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ auto command =
+ InstantCommand([] {}, {}).AndThen([&finished] { finished = true; });
+
+ scheduler.Schedule(&command);
+
+ EXPECT_FALSE(finished);
+
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ EXPECT_TRUE(finished);
+}
+
+TEST_F(CommandDecoratorTest, PerpetuallyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ auto command = InstantCommand([] {}, {}).Perpetually();
+
+ scheduler.Schedule(&command);
+
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
new file mode 100644
index 0000000..2600430
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandRequirementsTest : public CommandTestBase {};
+
+TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement;
+
+ MockCommand command1({&requirement});
+ MockCommand command2({&requirement});
+
+ EXPECT_CALL(command1, Initialize());
+ EXPECT_CALL(command1, Execute());
+ EXPECT_CALL(command1, End(true));
+ EXPECT_CALL(command1, End(false)).Times(0);
+
+ EXPECT_CALL(command2, Initialize());
+ EXPECT_CALL(command2, Execute());
+ EXPECT_CALL(command2, End(true)).Times(0);
+ EXPECT_CALL(command2, End(false)).Times(0);
+
+ scheduler.Schedule(&command1);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ scheduler.Schedule(&command2);
+ EXPECT_FALSE(scheduler.IsScheduled(&command1));
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+ scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement;
+
+ MockCommand command1({&requirement});
+ MockCommand command2({&requirement});
+
+ EXPECT_CALL(command1, Initialize());
+ EXPECT_CALL(command1, Execute()).Times(2);
+ EXPECT_CALL(command1, End(true)).Times(0);
+ EXPECT_CALL(command1, End(false)).Times(0);
+
+ EXPECT_CALL(command2, Initialize()).Times(0);
+ EXPECT_CALL(command2, Execute()).Times(0);
+ EXPECT_CALL(command2, End(true)).Times(0);
+ EXPECT_CALL(command2, End(false)).Times(0);
+
+ scheduler.Schedule(false, &command1);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ scheduler.Schedule(&command2);
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ EXPECT_FALSE(scheduler.IsScheduled(&command2));
+ scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
+ TestSubsystem requirement1;
+ ErrorConfirmer confirmer("require");
+
+ MockCommand command1;
+
+ requirement1.SetDefaultCommand(std::move(command1));
+
+ EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
new file mode 100644
index 0000000..6572a98
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+class CommandScheduleTest : public CommandTestBase {};
+
+TEST_F(CommandScheduleTest, InstantScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_CALL(command, Initialize());
+ EXPECT_CALL(command, Execute());
+ EXPECT_CALL(command, End(false));
+
+ command.SetFinished(true);
+ scheduler.Schedule(&command);
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, SingleIterationScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_CALL(command, Initialize());
+ EXPECT_CALL(command, Execute()).Times(2);
+ EXPECT_CALL(command, End(false));
+
+ scheduler.Schedule(&command);
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ scheduler.Run();
+ command.SetFinished(true);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, MultiScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command1;
+ MockCommand command2;
+ MockCommand command3;
+
+ EXPECT_CALL(command1, Initialize());
+ EXPECT_CALL(command1, Execute()).Times(2);
+ EXPECT_CALL(command1, End(false));
+
+ EXPECT_CALL(command2, Initialize());
+ EXPECT_CALL(command2, Execute()).Times(3);
+ EXPECT_CALL(command2, End(false));
+
+ EXPECT_CALL(command3, Initialize());
+ EXPECT_CALL(command3, Execute()).Times(4);
+ EXPECT_CALL(command3, End(false));
+
+ scheduler.Schedule(&command1);
+ scheduler.Schedule(&command2);
+ scheduler.Schedule(&command3);
+ EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+ command1.SetFinished(true);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3}));
+ EXPECT_FALSE(scheduler.IsScheduled(&command1));
+ command2.SetFinished(true);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2}));
+ command3.SetFinished(true);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
+}
+
+TEST_F(CommandScheduleTest, SchedulerCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_CALL(command, Initialize());
+ EXPECT_CALL(command, Execute());
+ EXPECT_CALL(command, End(false)).Times(0);
+ EXPECT_CALL(command, End(true));
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ scheduler.Cancel(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, NotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
new file mode 100644
index 0000000..0429c62
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+
+CommandTestBase::CommandTestBase() {
+ auto& scheduler = CommandScheduler::GetInstance();
+ scheduler.CancelAll();
+ scheduler.Enable();
+ scheduler.ClearButtons();
+}
+
+CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
+
+void CommandTestBase::SetUp() {
+ HALSIM_SetDriverStationEnabled(true);
+ while (!HALSIM_GetDriverStationEnabled()) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+}
+
+void CommandTestBase::TearDown() {
+ CommandScheduler::GetInstance().ClearButtons();
+}
+
+void CommandTestBase::SetDSEnabled(bool enabled) {
+ HALSIM_SetDriverStationEnabled(enabled);
+ while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
new file mode 100644
index 0000000..8b22844
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <regex>
+#include <utility>
+
+#include <mockdata/MockHooks.h>
+
+#include "ErrorConfirmer.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/SetUtilities.h"
+#include "frc2/command/SubsystemBase.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "make_vector.h"
+#include "simulation/DriverStationSim.h"
+
+namespace frc2 {
+class CommandTestBase : public ::testing::Test {
+ public:
+ CommandTestBase();
+
+ class TestSubsystem : public SubsystemBase {};
+
+ protected:
+ class MockCommand : public Command {
+ public:
+ MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
+ MOCK_METHOD0(IsFinished, bool());
+ MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
+ MOCK_METHOD0(Initialize, void());
+ MOCK_METHOD0(Execute, void());
+ MOCK_METHOD1(End, void(bool interrupted));
+
+ MockCommand() {
+ m_requirements = {};
+ EXPECT_CALL(*this, GetRequirements())
+ .WillRepeatedly(::testing::Return(m_requirements));
+ EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
+ EXPECT_CALL(*this, RunsWhenDisabled())
+ .WillRepeatedly(::testing::Return(true));
+ }
+
+ MockCommand(std::initializer_list<Subsystem*> requirements,
+ bool finished = false, bool runWhenDisabled = true) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+ EXPECT_CALL(*this, GetRequirements())
+ .WillRepeatedly(::testing::Return(m_requirements));
+ EXPECT_CALL(*this, IsFinished())
+ .WillRepeatedly(::testing::Return(finished));
+ EXPECT_CALL(*this, RunsWhenDisabled())
+ .WillRepeatedly(::testing::Return(runWhenDisabled));
+ }
+
+ MockCommand(MockCommand&& other) {
+ EXPECT_CALL(*this, IsFinished())
+ .WillRepeatedly(::testing::Return(other.IsFinished()));
+ EXPECT_CALL(*this, RunsWhenDisabled())
+ .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
+ std::swap(m_requirements, other.m_requirements);
+ EXPECT_CALL(*this, GetRequirements())
+ .WillRepeatedly(::testing::Return(m_requirements));
+ }
+
+ MockCommand(const MockCommand& other) : Command{} {}
+
+ void SetFinished(bool finished) {
+ EXPECT_CALL(*this, IsFinished())
+ .WillRepeatedly(::testing::Return(finished));
+ }
+
+ ~MockCommand() {
+ auto& scheduler = CommandScheduler::GetInstance();
+ scheduler.Cancel(this);
+ }
+
+ protected:
+ std::unique_ptr<Command> TransferOwnership() && {
+ return std::make_unique<MockCommand>(std::move(*this));
+ }
+
+ private:
+ wpi::SmallSet<Subsystem*, 4> m_requirements;
+ };
+
+ CommandScheduler GetScheduler();
+
+ void SetUp() override;
+
+ void TearDown() override;
+
+ void SetDSEnabled(bool enabled);
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
new file mode 100644
index 0000000..927721a
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class ConditionalCommandTest : public CommandTestBase {};
+
+TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+ MockCommand* mockptr = mock.get();
+
+ EXPECT_CALL(*mock, Initialize());
+ EXPECT_CALL(*mock, Execute()).Times(2);
+ EXPECT_CALL(*mock, End(false));
+
+ ConditionalCommand conditional(
+ std::move(mock), std::make_unique<InstantCommand>(), [] { return true; });
+
+ scheduler.Schedule(&conditional);
+ scheduler.Run();
+ mockptr->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
+
+TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ConditionalCommand conditional(std::move(command1), std::move(command2),
+ [] { return true; });
+ scheduler.Schedule(&conditional);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
new file mode 100644
index 0000000..b97cbb6
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class DefaultCommandTest : public CommandTestBase {};
+
+TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem subsystem;
+
+ RunCommand command1([] {}, {&subsystem});
+
+ scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+ auto handle = scheduler.GetDefaultCommand(&subsystem);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
+
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem subsystem;
+
+ RunCommand command1([] {}, {&subsystem});
+ RunCommand command2([] {}, {&subsystem});
+
+ scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+ auto handle = scheduler.GetDefaultCommand(&subsystem);
+ scheduler.Run();
+ scheduler.Schedule(&command2);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+ EXPECT_FALSE(scheduler.IsScheduled(handle));
+
+ scheduler.Cancel(&command2);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
new file mode 100644
index 0000000..2565a94
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "ErrorConfirmer.h"
+
+ErrorConfirmer* ErrorConfirmer::instance;
+
+int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
+ HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack,
+ HAL_Bool printMsg) {
+ if (std::regex_search(details, std::regex(instance->m_msg))) {
+ instance->ConfirmError();
+ }
+ return 1;
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
new file mode 100644
index 0000000..011158c
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <regex>
+
+#include <mockdata/MockHooks.h>
+
+#include "gmock/gmock.h"
+#include "simulation/DriverStationSim.h"
+
+class ErrorConfirmer {
+ public:
+ explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
+ if (instance != nullptr) return;
+ HALSIM_SetSendError(HandleError);
+ EXPECT_CALL(*this, ConfirmError());
+ instance = this;
+ }
+
+ ~ErrorConfirmer() {
+ HALSIM_SetSendError(nullptr);
+ instance = nullptr;
+ }
+
+ MOCK_METHOD0(ConfirmError, void());
+
+ const char* m_msg;
+
+ static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
+ HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack,
+ HAL_Bool printMsg);
+
+ private:
+ static ErrorConfirmer* instance;
+};
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
new file mode 100644
index 0000000..140d4fb
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class FunctionalCommandTest : public CommandTestBase {};
+
+TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+ bool finished = false;
+
+ FunctionalCommand command(
+ [&counter] { counter++; }, [&counter] { counter++; },
+ [&counter](bool) { counter++; }, [&finished] { return finished; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ EXPECT_EQ(4, counter);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
new file mode 100644
index 0000000..e91f677
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class InstantCommandTest : public CommandTestBase {};
+
+TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ InstantCommand command([&counter] { counter++; }, {});
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ EXPECT_EQ(counter, 1);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
new file mode 100644
index 0000000..4eae880
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/Subsystem.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class MecanumControllerCommandTest : public ::testing::Test {
+ using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+ protected:
+ frc2::Timer m_timer;
+ frc::Rotation2d m_angle{0_rad};
+
+ units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
+ units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
+ units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
+ units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
+
+ frc::ProfiledPIDController<units::radians> m_rotController{
+ 1, 0, 0,
+ frc::TrapezoidProfile<units::radians>::Constraints{
+ 9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+ static constexpr units::meter_t kxTolerance{1 / 12.0};
+ static constexpr units::meter_t kyTolerance{1 / 12.0};
+ static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+ static constexpr units::meter_t kWheelBase{0.5};
+ static constexpr units::meter_t kTrackWidth{0.5};
+
+ frc::MecanumDriveKinematics m_kinematics{
+ frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+ frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
+ frc::Pose2d{0_m, 0_m, 0_rad}};
+
+ frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+ return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
+ m_rearLeftSpeed, m_rearRightSpeed};
+ }
+
+ frc::Pose2d getRobotPose() {
+ m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+ return m_odometry.GetPose();
+ }
+};
+
+TEST_F(MecanumControllerCommandTest, ReachesReference) {
+ frc2::Subsystem subsystem{};
+
+ auto waypoints =
+ std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ auto endState = trajectory.Sample(trajectory.TotalTime());
+
+ auto command = frc2::MecanumControllerCommand(
+ trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+ frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+ m_rotController, units::meters_per_second_t(8.8),
+ [&](units::meters_per_second_t frontLeft,
+ units::meters_per_second_t rearLeft,
+ units::meters_per_second_t frontRight,
+ units::meters_per_second_t rearRight) {
+ m_frontLeftSpeed = frontLeft;
+ m_rearLeftSpeed = rearLeft;
+ m_frontRightSpeed = frontRight;
+ m_rearRightSpeed = rearRight;
+ },
+ {&subsystem});
+
+ m_timer.Reset();
+ m_timer.Start();
+ command.Initialize();
+ while (!command.IsFinished()) {
+ command.Execute();
+ m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+ }
+ m_timer.Stop();
+ command.End(false);
+
+ EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+ getRobotPose().Translation().X(), kxTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+ getRobotPose().Translation().Y(), kyTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+ getRobotPose().Rotation().Radians(), kAngularTolerance);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
new file mode 100644
index 0000000..a34c292
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+class NotifierCommandTest : public CommandTestBase {};
+
+#ifdef __APPLE__
+TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
+#else
+TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
+#endif
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ NotifierCommand command([&counter] { counter++; }, 0.01_s, {});
+
+ scheduler.Schedule(&command);
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+ scheduler.Cancel(&command);
+
+ EXPECT_GT(counter, 10);
+ EXPECT_LT(counter, 100);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
new file mode 100644
index 0000000..55c7b98
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelCommandGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+
+ ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(2);
+ EXPECT_CALL(*command2, End(false));
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ command2->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+
+ ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(2);
+ EXPECT_CALL(*command2, End(false)).Times(0);
+ EXPECT_CALL(*command2, End(true));
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ scheduler.Run();
+ scheduler.Cancel(&group);
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelCommandGroup group((InstantCommand(), InstantCommand()));
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ ParallelCommandGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ParallelCommandGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
new file mode 100644
index 0000000..6e3246f
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelDeadlineGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelDeadlineGroup group(
+ std::move(command1Holder),
+ tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+ std::move(command3Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(2);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(false));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(2);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ command2->SetFinished(true);
+ scheduler.Run();
+ command1->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem subsystem;
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelDeadlineGroup group(
+ std::move(command1Holder),
+ tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+ std::move(command3Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(true));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(true));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(1);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ scheduler.Run();
+ scheduler.Cancel(&group);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ ParallelDeadlineGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ParallelDeadlineGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
new file mode 100644
index 0000000..54762ca
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelRaceGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(2);
+ EXPECT_CALL(*command1, End(true));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(2);
+ EXPECT_CALL(*command2, End(false));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(2);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ scheduler.Run();
+ command2->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(true));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(true));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(1);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ scheduler.Run();
+ scheduler.Cancel(&group);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelRaceGroup group{InstantCommand(), InstantCommand()};
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ ParallelRaceGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ParallelRaceGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished1 = false;
+ bool finished2 = false;
+ bool finished3 = false;
+
+ WaitUntilCommand command1([&finished1] { return finished1; });
+ WaitUntilCommand command2([&finished2] { return finished2; });
+ WaitUntilCommand command3([&finished3] { return finished3; });
+
+ SequentialCommandGroup group1(command1, command2);
+ ParallelRaceGroup group2(std::move(group1), command3);
+
+ scheduler.Schedule(&group2);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group2));
+ finished1 = true;
+ scheduler.Run();
+ finished2 = true;
+ EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+ EXPECT_FALSE(scheduler.IsScheduled(&group2));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
new file mode 100644
index 0000000..b3ef861
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+class PerpetualCommandTest : public CommandTestBase {};
+
+TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool check = false;
+
+ PerpetualCommand command{InstantCommand([&check] { check = true; }, {})};
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ EXPECT_TRUE(check);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
new file mode 100644
index 0000000..b940566
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+class PrintCommandTest : public CommandTestBase {};
+
+TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ PrintCommand command("Test!");
+
+ testing::internal::CaptureStdout();
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(),
+ std::regex("Test!")));
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
new file mode 100644
index 0000000..09a569f
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ProxyScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
+ CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+ bool scheduled = false;
+
+ InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+ ProxyScheduleCommand command(&toSchedule);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
+ CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+ bool finished = false;
+
+ WaitUntilCommand toSchedule([&finished] { return finished; });
+
+ ProxyScheduleCommand command(&toSchedule);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
new file mode 100644
index 0000000..bac40d5
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class RobotDisabledCommandTest : public CommandTestBase {};
+
+TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ MockCommand command({}, false, false);
+
+ EXPECT_CALL(command, End(true));
+
+ SetDSEnabled(true);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+ SetDSEnabled(false);
+
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ MockCommand command1;
+ MockCommand command2;
+
+ scheduler.Schedule(&command1);
+
+ SetDSEnabled(false);
+
+ scheduler.Run();
+
+ scheduler.Schedule(&command2);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+ SequentialCommandGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+ ParallelCommandGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
+ ParallelRaceGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
+ ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
+ [] { return true; }};
+ ConditionalCommand dontRunWhenDisabled{
+ MockCommand(), MockCommand({}, false, false), [] { return true; }};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ SelectCommand<int> runWhenDisabled{[] { return 1; },
+ std::pair(1, MockCommand()),
+ std::pair(1, MockCommand())};
+ SelectCommand<int> dontRunWhenDisabled{
+ [] { return 1; }, std::pair(1, MockCommand()),
+ std::pair(1, MockCommand({}, false, false))};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
new file mode 100644
index 0000000..07eecb3
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class RunCommandTest : public CommandTestBase {};
+
+TEST_F(RunCommandTest, RunCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ RunCommand command([&counter] { counter++; }, {});
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_EQ(3, counter);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
new file mode 100644
index 0000000..29e8dc5
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class ScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
+ CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+ bool scheduled = false;
+
+ InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+ ScheduleCommand command(&toSchedule);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduled);
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
new file mode 100644
index 0000000..f0198c9
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class SchedulerTest : public CommandTestBase {};
+
+TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
+ CommandScheduler scheduler = GetScheduler();
+
+ InstantCommand command;
+
+ int counter = 0;
+
+ scheduler.OnCommandInitialize([&counter](const Command&) { counter++; });
+ scheduler.OnCommandExecute([&counter](const Command&) { counter++; });
+ scheduler.OnCommandFinish([&counter](const Command&) { counter++; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_EQ(counter, 3);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ RunCommand command([] {}, {});
+
+ int counter = 0;
+
+ scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ scheduler.Cancel(&command);
+
+ EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, UnregisterSubsystemTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem system;
+
+ scheduler.RegisterSubsystem(&system);
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
new file mode 100644
index 0000000..6be14ef
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class SelectCommandTest : public CommandTestBase {};
+
+TEST_F(SelectCommandTest, SelectCommandTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+ MockCommand* mockptr = mock.get();
+
+ EXPECT_CALL(*mock, Initialize());
+ EXPECT_CALL(*mock, Execute()).Times(2);
+ EXPECT_CALL(*mock, End(false));
+
+ std::vector<std::pair<int, std::unique_ptr<Command>>> temp;
+
+ temp.emplace_back(std::pair(1, std::move(mock)));
+ temp.emplace_back(std::pair(2, std::make_unique<InstantCommand>()));
+ temp.emplace_back(std::pair(3, std::make_unique<InstantCommand>()));
+
+ SelectCommand<int> select([] { return 1; }, std::move(temp));
+
+ scheduler.Schedule(&select);
+ scheduler.Run();
+ mockptr->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
+
+TEST_F(SelectCommandTest, SelectCommandRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ SelectCommand<int> select([] { return 1; }, std::pair(1, std::move(command1)),
+ std::pair(2, std::move(command2)));
+
+ scheduler.Schedule(&select);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
new file mode 100644
index 0000000..3a6f8d8
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class SequentialCommandGroupTest : public CommandTestBase {};
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(false));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(1);
+ EXPECT_CALL(*command3, End(false));
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ command2->SetFinished(true);
+ scheduler.Run();
+ command3->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(0);
+ EXPECT_CALL(*command2, End(false)).Times(0);
+ EXPECT_CALL(*command2, End(true));
+
+ EXPECT_CALL(*command3, Initialize()).Times(0);
+ EXPECT_CALL(*command3, Execute()).Times(0);
+ EXPECT_CALL(*command3, End(false)).Times(0);
+ EXPECT_CALL(*command3, End(true)).Times(0);
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ scheduler.Cancel(&group);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ SequentialCommandGroup group{InstantCommand(), InstantCommand()};
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ SequentialCommandGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ SequentialCommandGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
new file mode 100644
index 0000000..3f25792
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+class StartEndCommandTest : public CommandTestBase {};
+
+TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; },
+ {});
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ scheduler.Run();
+ scheduler.Cancel(&command);
+
+ EXPECT_EQ(2, counter);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
new file mode 100644
index 0000000..fa0838d
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/Subsystem.h>
+#include <frc2/command/SwerveControllerCommand.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class SwerveControllerCommandTest : public ::testing::Test {
+ using radians_per_second_squared_t =
+ units::compound_unit<units::radians,
+ units::inverse<units::squared<units::second>>>;
+
+ protected:
+ frc2::Timer m_timer;
+ frc::Rotation2d m_angle{0_rad};
+
+ std::array<frc::SwerveModuleState, 4> m_moduleStates{
+ frc::SwerveModuleState{}, frc::SwerveModuleState{},
+ frc::SwerveModuleState{}, frc::SwerveModuleState{}};
+
+ frc::ProfiledPIDController<units::radians> m_rotController{
+ 1, 0, 0,
+ frc::TrapezoidProfile<units::radians>::Constraints{
+ 9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+ static constexpr units::meter_t kxTolerance{1 / 12.0};
+ static constexpr units::meter_t kyTolerance{1 / 12.0};
+ static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+ static constexpr units::meter_t kWheelBase{0.5};
+ static constexpr units::meter_t kTrackWidth{0.5};
+
+ frc::SwerveDriveKinematics<4> m_kinematics{
+ frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+ frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+ frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
+ frc::Pose2d{0_m, 0_m, 0_rad}};
+
+ std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
+ return m_moduleStates;
+ }
+
+ frc::Pose2d getRobotPose() {
+ m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+ return m_odometry.GetPose();
+ }
+};
+
+TEST_F(SwerveControllerCommandTest, ReachesReference) {
+ frc2::Subsystem subsystem{};
+
+ auto waypoints =
+ std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ auto endState = trajectory.Sample(trajectory.TotalTime());
+
+ auto command = frc2::SwerveControllerCommand<4>(
+ trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+ frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+ m_rotController,
+ [&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem});
+
+ m_timer.Reset();
+ m_timer.Start();
+ command.Initialize();
+ while (!command.IsFinished()) {
+ command.Execute();
+ m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+ }
+ m_timer.Stop();
+ command.End(false);
+
+ EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+ getRobotPose().Translation().X(), kxTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+ getRobotPose().Translation().Y(), kyTolerance);
+ EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+ getRobotPose().Rotation().Radians(), kAngularTolerance);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
new file mode 100644
index 0000000..75841a6
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitCommandTest : public CommandTestBase {};
+
+TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ WaitCommand command(100_ms);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ std::this_thread::sleep_for(std::chrono::milliseconds(110));
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
new file mode 100644
index 0000000..e9728db
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitUntilCommandTest : public CommandTestBase {};
+
+TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
new file mode 100644
index 0000000..89d55b6
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+namespace tcb {
+
+namespace detail {
+
+template <typename T, typename...>
+struct vec_type_helper {
+ using type = T;
+};
+
+template <typename... Args>
+struct vec_type_helper<void, Args...> {
+ using type = typename std::common_type_t<Args...>;
+};
+
+template <typename T, typename... Args>
+using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
+
+template <typename, typename...>
+struct all_constructible_and_convertible : std::true_type {};
+
+template <typename T, typename First, typename... Rest>
+struct all_constructible_and_convertible<T, First, Rest...>
+ : std::conditional_t<
+ std::is_constructible_v<T, First> && std::is_convertible_v<First, T>,
+ all_constructible_and_convertible<T, Rest...>, std::false_type> {};
+
+template <typename T, typename... Args,
+ typename std::enable_if_t<!std::is_trivially_copyable_v<T>, int> = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+ std::vector<T> vec;
+ vec.reserve(sizeof...(Args));
+ using arr_t = int[];
+ (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+ return vec;
+}
+
+template <typename T, typename... Args,
+ typename std::enable_if_t<std::is_trivially_copyable_v<T>, int> = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+ return std::vector<T>{std::forward<Args>(args)...};
+}
+
+} // namespace detail
+
+template <
+ typename T = void, typename... Args,
+ typename V = detail::vec_type_helper_t<T, Args...>,
+ typename std::enable_if_t<
+ detail::all_constructible_and_convertible<V, Args...>::value, int> = 0>
+std::vector<V> make_vector(Args&&... args) {
+ return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+}
+
+} // namespace tcb
diff --git a/wpilibNewCommands/src/test/native/cpp/main.cpp b/wpilibNewCommands/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..c6b6c58
--- /dev/null
+++ b/wpilibNewCommands/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/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension b/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
new file mode 100644
index 0000000..daaab59
--- /dev/null
+++ b/wpilibNewCommands/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension
@@ -0,0 +1 @@
+edu.wpi.first.wpilibj2.MockHardwareExtension