Add dependency injection of Subsystems to commands

wpilibc/src/main/native/cpp/commands/Command.cpp

@@ -27,6 +27,10 @@ Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
 
 Command::Command(double timeout) : Command("", timeout) {}
 
+Command::Command(Subsystem& requirement) : Command("", -1.0) {
+  Requires(&requirement);
+}
+
 Command::Command(const wpi::Twine& name, double timeout) : SendableBase(false) {
   // We use -1.0 to indicate no timeout.
   if (timeout < 0.0 && timeout != -1.0)
@@ -43,6 +47,21 @@ Command::Command(const wpi::Twine& name, double timeout) : SendableBase(false) {
   }
 }
 
+Command::Command(const wpi::Twine& name, Subsystem& requirement)
+    : Command(name, -1.0) {
+  Requires(&requirement);
+}
+
+Command::Command(double timeout, Subsystem& requirement)
+    : Command("", timeout) {
+  Requires(&requirement);
+}
+
+Command::Command(const wpi::Twine& name, double timeout, Subsystem& requirement)
+    : Command(name, timeout) {
+  Requires(&requirement);
+}
+
 double Command::TimeSinceInitialized() const {
   if (m_startTime < 0.0)
     return 0.0;

wpilibc/src/main/native/cpp/commands/InstantCommand.cpp

@@ -11,4 +11,7 @@ using namespace frc;
 
 InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
 
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
+    : Command(name, subsystem) {}
+
 bool InstantCommand::IsFinished() { return true; }

wpilibc/src/main/native/cpp/commands/PIDCommand.cpp

@@ -41,6 +41,39 @@ PIDCommand::PIDCommand(double p, double i, double d, double period) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
 }
 
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       double f, double period, Subsystem& requirement)
+    : Command(name, requirement) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
+                       Subsystem& requirement) {
+  m_controller =
+      std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       Subsystem& requirement)
+    : Command(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+                       double period, Subsystem& requirement)
+    : Command(name) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, Subsystem& requirement) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period,
+                       Subsystem& requirement) {
+  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
 void PIDCommand::_Initialize() { m_controller->Enable(); }
 
 void PIDCommand::_End() { m_controller->Disable(); }

wpilibc/src/main/native/cpp/commands/TimedCommand.cpp

@@ -14,4 +14,11 @@ TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
 
 TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
 
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
+                           Subsystem& requirement)
+    : Command(name, timeout, requirement) {}
+
+TimedCommand::TimedCommand(double timeout, Subsystem& requirement)
+    : Command(timeout, requirement) {}
+
 bool TimedCommand::IsFinished() { return IsTimedOut(); }

wpilibc/src/main/native/include/frc/commands/Command.h

@@ -72,6 +72,13 @@ class Command : public ErrorBase, public SendableBase {
    */
   explicit Command(double timeout);
 
+  /**
+   * Creates a new command with the given timeout and a default name.
+   *
+   * @param requirement the subsystem that the command requires
+   */
+  explicit Command(Subsystem& requirement);
+
   /**
    * Creates a new command with the given name and timeout.
    *
@@ -81,6 +88,33 @@ class Command : public ErrorBase, public SendableBase {
    */
   Command(const wpi::Twine& name, double timeout);
 
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param name        the name of the command
+   * @param requirement the subsystem that the command requires
+   */
+  Command(const wpi::Twine& name, Subsystem& requirement);
+
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param timeout     the time (in seconds) before this command "times out"
+   * @param requirement the subsystem that the command requires
+   * @see IsTimedOut()
+   */
+  Command(double timeout, Subsystem& requirement);
+
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param name        the name of the command
+   * @param timeout     the time (in seconds) before this command "times out"
+   * @param requirement the subsystem that the command requires
+   * @see IsTimedOut()
+   */
+  Command(const wpi::Twine& name, double timeout, Subsystem& requirement);
+
   ~Command() override = default;
 
   /**

wpilibc/src/main/native/include/frc/commands/InstantCommand.h

@@ -27,6 +27,14 @@ class InstantCommand : public Command {
    */
   explicit InstantCommand(const wpi::Twine& name);
 
+  /**
+   * Creates a new InstantCommand with the given name.
+   *
+   * @param name        The name for this command
+   * @param requirement The subsystem that the command requires
+   */
+  InstantCommand(const wpi::Twine& name, Subsystem& requirement);
+
   InstantCommand() = default;
   virtual ~InstantCommand() = default;
 

wpilibc/src/main/native/include/frc/commands/PIDCommand.h

@@ -28,6 +28,17 @@ class PIDCommand : public Command, public PIDOutput, public PIDSource {
   PIDCommand(double p, double i, double d);
   PIDCommand(double p, double i, double d, double period);
   PIDCommand(double p, double i, double d, double f, double period);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+             Subsystem& requirement);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+             double period, Subsystem& requirement);
+  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+             double period, Subsystem& requirement);
+  PIDCommand(double p, double i, double d, Subsystem& requirement);
+  PIDCommand(double p, double i, double d, double period,
+             Subsystem& requirement);
+  PIDCommand(double p, double i, double d, double f, double period,
+             Subsystem& requirement);
   virtual ~PIDCommand() = default;
 
   void SetSetpointRelative(double deltaSetpoint);

wpilibc/src/main/native/include/frc/commands/TimedCommand.h

@@ -35,6 +35,23 @@ class TimedCommand : public Command {
    */
   explicit TimedCommand(double timeout);
 
+  /**
+   * Creates a new TimedCommand with the given name and timeout.
+   *
+   * @param name        the name of the command
+   * @param timeout     the time (in seconds) before this command "times out"
+   * @param requirement the subsystem that the command requires
+   */
+  TimedCommand(const wpi::Twine& name, double timeout, Subsystem& requirement);
+
+  /**
+   * Creates a new WaitCommand with the given timeout.
+   *
+   * @param timeout     the time (in seconds) before this command "times out"
+   * @param requirement the subsystem that the command requires
+   */
+  TimedCommand(double timeout, Subsystem& requirement);
+
   virtual ~TimedCommand() = default;
 
  protected:

wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java

@@ -135,6 +135,53 @@ public Command(double timeout) {
     m_timeout = timeout;
   }
 
+  /**
+   * Creates a new command with the given timeout and a default name. The default name is the name
+   * of the class.
+   *
+   * @param requirement the subsystem that this command requires
+   * @throws IllegalArgumentException if given a negative timeout
+   * @see Command#isTimedOut() isTimedOut()
+   */
+  public Command(Subsystem requirement) {
+    this();
+    requires(requirement);
+  }
+
+  /**
+   * Creates a new command with the given name.
+   *
+   * @param name        the name for this command
+   * @param requirement the subsystem that this command requires
+   * @throws IllegalArgumentException if name is null
+   */
+  public Command(String name, Subsystem requirement) {
+    super(false);
+    if (name == null) {
+      throw new IllegalArgumentException("Name must not be null.");
+    }
+    setName(name);
+    requires(requirement);
+  }
+
+  /**
+   * Creates a new command with the given timeout and a default name. The default name is the name
+   * of the class.
+   *
+   * @param timeout     the time (in seconds) before this command "times out"
+   * @param requirement the subsystem that this command requires
+   * @throws IllegalArgumentException if given a negative timeout
+   * @see Command#isTimedOut() isTimedOut()
+   */
+  public Command(double timeout, Subsystem requirement) {
+    this();
+    if (timeout < 0) {
+      throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
+    }
+    m_timeout = timeout;
+    requires(requirement);
+  }
+
   /**
    * Creates a new command with the given name and timeout.
    *
@@ -151,6 +198,25 @@ public Command(String name, double timeout) {
     m_timeout = timeout;
   }
 
+  /**
+   * Creates a new command with the given name and timeout.
+   *
+   * @param name    the name of the command
+   * @param timeout the time (in seconds) before this command "times out"
+   * @param requirement the subsystem that this command requires
+   * @throws IllegalArgumentException if given a negative timeout
+   * @throws IllegalArgumentException if given a negative timeout or name was null.
+   * @see Command#isTimedOut() isTimedOut()
+   */
+  public Command(String name, double timeout, Subsystem requirement) {
+    this(name);
+    if (timeout < 0) {
+      throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
+    }
+    m_timeout = timeout;
+    requires(requirement);
+  }
+
   /**
    * Sets the timeout of this command.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java

@@ -25,6 +25,23 @@ public InstantCommand(String name) {
     super(name);
   }
 
+  /**
+   * Creates a new {@link InstantCommand InstantCommand} with the given requirement.
+   * @param requirement the subsystem this command requires
+   */
+  public InstantCommand(Subsystem requirement) {
+    super(requirement);
+  }
+
+  /**
+   * Creates a new {@link InstantCommand InstantCommand} with the given name and requirement.
+   * @param name        the name for this command
+   * @param requirement the subsystem this command requires
+   */
+  public InstantCommand(String name, Subsystem requirement) {
+    super(name, requirement);
+  }
+
   @Override
   protected boolean isFinished() {
     return true;

wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java

@@ -93,7 +93,7 @@ public PIDCommand(double p, double i, double d) {
 
   /**
    * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
-   * class name as its name.. It will also space the time between PID loop calculations to be equal
+   * class name as its name. It will also space the time between PID loop calculations to be equal
    * to the given period.
    *
    * @param p      the proportional value
@@ -106,6 +106,71 @@ public PIDCommand(double p, double i, double d, double period) {
     m_controller = new PIDController(p, i, d, m_source, m_output, period);
   }
 
+  /**
+   * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
+   *
+   * @param name        the name of the command
+   * @param p           the proportional value
+   * @param i           the integral value
+   * @param d           the derivative value
+   * @param requirement the subsystem that this command requires
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDCommand(String name, double p, double i, double d, Subsystem requirement) {
+    super(name, requirement);
+    m_controller = new PIDController(p, i, d, m_source, m_output);
+  }
+
+  /**
+   * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
+   * the time between PID loop calculations to be equal to the given period.
+   *
+   * @param name        the name
+   * @param p           the proportional value
+   * @param i           the integral value
+   * @param d           the derivative value
+   * @param period      the time (in seconds) between calculations
+   * @param requirement the subsystem that this command requires
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDCommand(String name, double p, double i, double d, double period,
+                    Subsystem requirement) {
+    super(name, requirement);
+    m_controller = new PIDController(p, i, d, m_source, m_output, period);
+  }
+
+  /**
+   * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
+   * class name as its name.
+   *
+   * @param p           the proportional value
+   * @param i           the integral value
+   * @param d           the derivative value
+   * @param requirement the subsystem that this command requires
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDCommand(double p, double i, double d, Subsystem requirement) {
+    super(requirement);
+    m_controller = new PIDController(p, i, d, m_source, m_output);
+  }
+
+  /**
+   * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
+   * class name as its name. It will also space the time between PID loop calculations to be equal
+   * to the given period.
+   *
+   * @param p           the proportional value
+   * @param i           the integral value
+   * @param d           the derivative value
+   * @param period      the time (in seconds) between calculations
+   * @param requirement the subsystem that this command requires
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDCommand(double p, double i, double d, double period, Subsystem requirement) {
+    super(requirement);
+    m_controller = new PIDController(p, i, d, m_source, m_output, period);
+  }
+
   /**
    * Returns the {@link PIDController} used by this {@link PIDCommand}. Use this if you would like
    * to fine tune the pid loop.