Turret angle tolerance checking

advantagescope/AdvantageScopeLayout.json

@@ -1,8 +1,8 @@
 {
   "hubs": [
     {
-      "x": -7,
-      "y": -7,
+      "x": 0,
+      "y": 116,
       "width": 1660,
       "height": 1063,
       "state": {
@@ -20,18 +20,19 @@
             "/RealOutputs/SwerveStates",
             "/RealOutputs/SwerveChassisSpeeds",
             "/RealOutputs/Odometry",
-            "/PowerDistribution"
+            "/PowerDistribution",
+            "/RealOutputs/Misc"
           ]
         },
         "tabs": {
-          "selected": 2,
+          "selected": 11,
           "tabs": [
             {
               "type": 0,
               "title": "",
               "controller": null,
               "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h",
-              "renderer": "#/tab-reference/joysticks",
+              "renderer": "",
               "controlsHeight": 0
             },
             {
@@ -71,9 +72,9 @@
                 "cameraIndex": -2,
                 "orbitFov": 50,
                 "cameraPosition": [
-                  -0.989579002734471,
+                  -0.9895790027344714,
                   1.424465471809841,
-                  0.6935410341631252
+                  0.6935410341631246
                 ],
                 "cameraTarget": [
                   0.1951088968028726,
@@ -496,7 +497,17 @@
                     }
                   }
                 ],
-                "discreteSources": [],
+                "discreteSources": [
+                  {
+                    "type": "stripes",
+                    "logKey": "/RealOutputs/Misc/Turret Within Tolerance",
+                    "logType": "Boolean",
+                    "visible": true,
+                    "options": {
+                      "color": "#80588e"
+                    }
+                  }
+                ],
                 "leftLockedRange": null,
                 "rightLockedRange": null,
                 "leftUnitConversion": {
@@ -551,6 +562,15 @@
                     "options": {
                       "color": "#2b66a2"
                     }
+                  },
+                  {
+                    "type": "stripes",
+                    "logKey": "/RealOutputs/Misc/Turret Within Tolerance",
+                    "logType": "Boolean",
+                    "visible": true,
+                    "options": {
+                      "color": "#80588e"
+                    }
                   }
                 ],
                 "leftLockedRange": null,

src/main/java/org/team157/robot/BuildConstants.java

@@ -5,13 +5,13 @@ public final class BuildConstants {
     public static final String MAVEN_GROUP = "";
     public static final String MAVEN_NAME = "FRC2026";
     public static final String VERSION = "unspecified";
-    public static final int GIT_REVISION = 444;
-    public static final String GIT_SHA = "1fbe3218ab8328d60c2133e518633d6f97069355";
-    public static final String GIT_DATE = "2026-06-10 17:07:32 EDT";
-    public static final String GIT_BRANCH = "task/update-advantagescope-layout";
-    public static final String BUILD_DATE = "2026-06-22 17:31:55 EDT";
-    public static final long BUILD_UNIX_TIME = 1782163915186L;
-    public static final int DIRTY = 0;
+    public static final int GIT_REVISION = 460;
+    public static final String GIT_SHA = "45b76112dba23d794e7cde47a6e4d6e09bb7f405";
+    public static final String GIT_DATE = "2026-06-22 19:12:47 EDT";
+    public static final String GIT_BRANCH = "feature/anti-slime";
+    public static final String BUILD_DATE = "2026-06-22 19:19:06 EDT";
+    public static final long BUILD_UNIX_TIME = 1782170346578L;
+    public static final int DIRTY = 1;
 
     private BuildConstants() {}
 }

src/main/java/org/team157/robot/RobotContainer.java

@@ -183,7 +183,7 @@ public RobotContainer() {
             hood.setIO(new HoodIO() {});
             slapdown.setIO(new SlapdownIO() {});
             hopper.setIO(new HopperIO() {});
-            uptake.setIO(new UptakeIO() {});
+            uptake.setIO(new UptakeIO() {}, turret);
             flywheel.setIO(new FlywheelIO() {}, vision);
             turret.setIO(new TurretIO() {}, vision);
         } else {
@@ -195,7 +195,7 @@ public RobotContainer() {
             hood.setIO(new HoodIOTalonFX(hood));
             slapdown.setIO(new SlapdownIOTalonFX(slapdown));
             hopper.setIO(new HopperIOTalonFX(hopper));
-            uptake.setIO(new UptakeIOTalonFX(uptake));
+            uptake.setIO(new UptakeIOTalonFX(uptake), turret);
             flywheel.setIO(new FlywheelIOTalonFX(flywheel), vision);
             turret.setIO(new TurretIOTalonFX(turret), vision);
         }
@@ -313,8 +313,6 @@ private void configureBindings() {
                                         drive)
                                 .ignoringDisable(true));
 
-
-
         /////////////////////
         /// FlYWHEEL HOOD ///
         /////////////////////
@@ -328,12 +326,11 @@ private void configureBindings() {
             driverController.b().onTrue(Commands.runOnce(drive::stopWithX, drive));
         }
 
-
         ////////////////////////////
         /// INTAKE UPTAKE HOPPER ///
         ////////////////////////////
 
-        driverController.rightTrigger().whileTrue(uptake.set(1));
+        driverController.rightTrigger().whileTrue(uptake.runUptake());
         driverController.rightTrigger().whileTrue(hopper.set(1));
 
         driverController.leftTrigger().whileTrue(intake.runIntake());

src/main/java/org/team157/robot/subsystems/turret/Turret.java

@@ -122,6 +122,24 @@ public void updateRelativeAngleToTarget(Pose2d targetPose, Pose2d robotPose) {
         }
     }
 
+    /**
+     * Determines whether the turret's current angle is within tolerance of its setpoint angle by a
+     * specified amount of degrees.
+     *
+     * @param thresholdDegrees The +- tolerance range to check, in degrees.
+     * @return true if the turret's angle is within the specified threshold in either direction,
+     *     false otherwise.
+     */
+    public boolean isWithinTolerance(double thresholdDegrees) {
+        if (inputs.targetAngleDegrees > inputs.angleDegrees) {
+            return inputs.targetAngleDegrees - inputs.angleDegrees < thresholdDegrees;
+        } else if (inputs.targetAngleDegrees < inputs.angleDegrees) {
+            return inputs.angleDegrees - inputs.targetAngleDegrees < thresholdDegrees;
+        } else {
+            return false;
+        }
+    }
+
     /**
      * Gets the 2D rotational pose (yaw) of the turret for mechanism visualization. Feeds into the
      * {@link SunstoneMechanism3D} class for the AdvantageScope model.
@@ -140,6 +158,7 @@ public Angle getTurretRotation() {
     public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Turret", inputs);
+        Logger.recordOutput("Misc/Turret Within Tolerance", isWithinTolerance(15));
     }
 
     @Override

src/main/java/org/team157/robot/subsystems/uptake/Uptake.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
+import org.team157.robot.subsystems.turret.Turret;
 
 /**
  * Represents the Uptake subsystem, which feeds balls from the hopper up into the flywheel for
@@ -12,6 +13,7 @@ public class Uptake extends SubsystemBase {
 
     // The IO interface for interacting with the uptake's motor.
     private UptakeIO io;
+    private Turret turret;
 
     // Inputs from the motor and mechanism, to be updated periodically and logged.
     private final UptakeIOInputsAutoLogged inputs = new UptakeIOInputsAutoLogged();
@@ -24,8 +26,9 @@ public Uptake() {}
      *
      * @param io An implementation of the Uptake's IO layer, i.e. UptakeIOTalonFX
      */
-    public void setIO(UptakeIO io) {
+    public void setIO(UptakeIO io, Turret turret) {
         this.io = io;
+        this.turret = turret;
     }
 
     /**
@@ -48,9 +51,32 @@ public Command set(double dutyCycle) {
         return io.set(dutyCycle);
     }
 
+    /**
+     * Determines the duty cycle setpoint of the uptake. Runs backwards when turret is not within
+     * tolerance, forwards otherwise.
+     */
+    public double getDutyCycleSetpoint() {
+        if (!turret.isWithinTolerance(15.7)) {
+            return UptakeConstants.RUN_SPEED_LOW;
+        } else {
+            return UptakeConstants.RUN_SPEED_HIGH;
+        }
+    }
+
+    /**
+     * Runs the uptake at a dynamic speed based on the current turret state, as determined in
+     * setDutyCycleSetpoint()
+     *
+     * @return a {@link Command} running the uptake at the desired speed
+     */
+    public Command runUptake() {
+        return io.set(this::getDutyCycleSetpoint);
+    }
+
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
+        // Update the uptake's setpoint based on the turret's tolerance status
         // Updates the inputs to be logged by AdvantageKit and writes them to the Logger
         io.updateInputs(inputs);
         Logger.processInputs("Uptake", inputs);

src/main/java/org/team157/robot/subsystems/uptake/UptakeConstants.java

@@ -11,7 +11,15 @@
 import yams.gearing.MechanismGearing;
 
 public final class UptakeConstants {
+    /** CAN IDs for both Uptake motors */
     public static final int MOTOR_ID = 18, FOLLOWER_MOTOR_ID = 19;
+    /** The current draw limit of the subsystem, in amps. */
     public static final Current CURRENT_LIMIT = Amps.of(40);
+    /** The gear ratio of the uptake subsystem. */
     public static final MechanismGearing GEARING = new MechanismGearing(GearBox.fromStages("9:1"));
+    /**
+     * The possible duty cycle outputs which the uptake can receive as its setpoint, depending on
+     * the turret state.
+     */
+    public static final double RUN_SPEED_HIGH = 1.0, RUN_SPEED_LOW = -0.157;
 }

src/main/java/org/team157/robot/subsystems/uptake/UptakeIO.java

@@ -2,6 +2,8 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLog;
 
 /**
@@ -53,4 +55,23 @@ default Command stop() {
     default Command set(double dutyCycle) {
         return Commands.none();
     }
+
+    /**
+     * Directly sets the output duty cycle of the uptake rollers' motors.
+     *
+     * @param dutyCycle The supplied duty cycle to apply to the motor, between -1 and 1.
+     * @return a {@link Command} setting the motor's duty cycle to the specified value.
+     */
+    default Command set(DoubleSupplier dutyCycle) {
+        return Commands.none();
+    }
+    /**
+     * Directly sets the output duty cycle of the uptake rollers' motors.
+     *
+     * @param dutyCycle The supplied duty cycle to apply to the motor, between -1 and 1.
+     * @return a {@link Command} setting the motor's duty cycle to the specified value.
+     */
+    default Command set(Supplier<Double> dutyCycle) {
+        return Commands.none();
+    }
 }

src/main/java/org/team157/robot/subsystems/uptake/UptakeIOTalonFX.java

@@ -12,6 +12,8 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 import org.team157.robot.Constants;
 import yams.mechanisms.config.FlyWheelConfig;
 import yams.mechanisms.velocity.FlyWheel;
@@ -73,6 +75,16 @@ public Command set(double dutyCycle) {
         return uptake.set(dutyCycle);
     }
 
+    @Override
+    public Command set(Supplier<Double> dutyCycle) {
+        return uptake.set(dutyCycle);
+    }
+
+    @Override
+    public Command set(DoubleSupplier dutyCycle) {
+        return set((Supplier<Double>) dutyCycle::getAsDouble);
+    }
+
     @Override
     public void simIterate() {
         uptake.simIterate();