Minor improvements/fixes to new command framework (#2186)

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java

@@ -25,10 +25,20 @@ public abstract class PIDSubsystem extends SubsystemBase {
    * Creates a new PIDSubsystem.
    *
    * @param controller the PIDController to use
+   * @param initialPosition the initial setpoint of the subsystem
+   */
+  public PIDSubsystem(PIDController controller, double initialPosition) {
+    setSetpoint(initialPosition);
+    m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+  }
+
+  /**
+   * Creates a new PIDSubsystem.  Initial setpoint is zero.
+   *
+   * @param controller the PIDController to use
    */
   public PIDSubsystem(PIDController controller) {
-    requireNonNullParam(controller, "controller", "PIDSubsystem");
-    m_controller = controller;
+    this(controller, 0);
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java

@@ -27,10 +27,21 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
    * Creates a new ProfiledPIDSubsystem.
    *
    * @param controller the ProfiledPIDController to use
+   * @param initialPosition the initial goal position of the controller
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller,
+                              double initialPosition) {
+    m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+    setGoal(initialPosition);
+  }
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.  Initial goal position is zero.
+   *
+   * @param controller the ProfiledPIDController to use
    */
   public ProfiledPIDSubsystem(ProfiledPIDController controller) {
-    requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
-    m_controller = controller;
+    this(controller, 0);
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java

@@ -9,6 +9,8 @@ package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 /**
  * 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.
@@ -20,41 +22,53 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
   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;
-  }
+  private boolean m_enabled = true;
 
   /**
    * 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
+   * @param initialPosition The initial position of the controlled 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_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
     m_state = new TrapezoidProfile.State(initialPosition, 0);
+    setGoal(initialPosition);
     m_period = period;
   }
 
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem
+   *                        is constructed.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
+                                   double initialPosition) {
+    this(constraints, initialPosition, 0.02);
+  }
+
+  /**
+   * Creates a new TrapezoidProfileSubsystem.
+   *
+   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   */
+  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
+    this(constraints, 0, 0.02);
+  }
+
   @Override
   public void periodic() {
     var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
     m_state = profile.calculate(m_period);
-    useState(m_state);
+    if (m_enabled) {
+      useState(m_state);
+    }
   }
 
   /**
@@ -75,6 +89,20 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
     setGoal(new TrapezoidProfile.State(goal, 0));
   }
 
+  /**
+   * Enable the TrapezoidProfileSubsystem's output.
+   */
+  public void enable() {
+    m_enabled = true;
+  }
+
+  /**
+   * Disable the TrapezoidProfileSubsystem's output.
+   */
+  public void disable() {
+    m_enabled = false;
+  }
+
   /**
    * Users should override this to consume the current state of the motion profile.
    *