From a69e462e614717df59c8d35161e9751701fb9730 Mon Sep 17 00:00:00 2001
From: Kevin Cooney <kcooney@gmail.com>
Date: Thu, 6 Nov 2025 19:45:38 -0800
Subject: [PATCH] Add isValidSetpoint()

---
 .../lib2813/subsystems/MotorSubsystem.java    | 29 +++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
index 9901c463..d1281cab 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
@@ -106,9 +106,16 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) {
   /**
    * Sets the desired setpoint to the provided value, and enables the PID control.
    *
+   * <p>This method will call {@link #isValidSetpoint(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 (!isValidSetpoint(position)) {
+      return;
+    }
+
     if (!isEnabled()) {
       enable();
     }
@@ -116,6 +123,28 @@ public final void setSetpoint(T position) {
     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 isValidSetpoint(T position) {
+    return true;
+  }
+
   /**
    * Returns a command that sets the desired setpoint to the provided value.
    *