Add isValidateSetpoint()

Author: kcooney

lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java

@@ -106,16 +106,46 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) {
   /**
    * Sets the desired setpoint to the provided value, and enables the PID control.
    *
+   * <p>This method will call {@link #isValidateSetpoint(Supplier)} before it makes any changes to
+   * this subsystem. If that method call returns {@code false} then this method will silently do
+   * nothing.
+   *
    * @param position the position to go to.
    */
   public final void setSetpoint(T position) {
+    if (!isValidateSetpoint(position)) {
+      return;
+    }
+
     if (!isEnabled()) {
       enable();
     }
     double setpoint = position.get().in(rotationUnit);
     controller.setSetpoint(setpoint);
   }
 
+  /**
+   * Determines if the given position is a valid setpoint, given the current state of the subsystem.
+   *
+   * <p>This is an extension point that allows subclasses to prevent unsafe movements. This method
+   * is called by {@link #setSetpoint(Supplier)} before that method makes any changes to the
+   * subsystem. Subclasses that want to prevent movement to positions that are unsafe can override
+   * this method and have it return {@code false} to indicate that {@link #setSetpoint(Supplier)}
+   * should silently ignore the request to change the setpoint.
+   *
+   * <p>Subclasses that override this method <em>may</em> choose to report a warning to the user
+   * before returning {@code false}, but <em>should not</em> perform any actions that would lead to
+   * an observable behavior change on this subsystem.
+   *
+   * <p>The default implementation returns {@code true}.
+   *
+   * @param position the position passed to {@link #setSetpoint(Supplier)}.
+   * @return {@code true} if the position should be considered valid, otherwise {@code false}.
+   */
+  protected boolean isValidateSetpoint(T position) {
+    return true;
+  }
+
   /**
    * Returns a command that sets the desired setpoint to the provided value.
    *