Add new C++ Command framework (#1785)

This is the C++ version of #1682.

The old command framework is still available, but will be deprecated.

Due to name conflicts, the new framework is in the frc2 namespace.
Eventually (after the old command framework is removed in a future year)
it will be moved into the main frc namespace.

wpilibc/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 <iostream>
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "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<Command, std::remove_reference_t<T1>>::value>,
+            typename = std::enable_if_t<
+                std::is_base_of<Command, std::remove_reference_t<T2>>::value>>
+  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