diff --git a/advantagescope/AdvantageScopeLayout.json b/advantagescope/AdvantageScopeLayout.json index e009e92..5c83e28 100644 --- a/advantagescope/AdvantageScopeLayout.json +++ b/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, diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 2e0fd52..ec81a0d 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/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() {} } diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index f50fa02..c7e9563 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/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()); diff --git a/src/main/java/org/team157/robot/subsystems/turret/Turret.java b/src/main/java/org/team157/robot/subsystems/turret/Turret.java index 107c336..2ec7af4 100644 --- a/src/main/java/org/team157/robot/subsystems/turret/Turret.java +++ b/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 diff --git a/src/main/java/org/team157/robot/subsystems/uptake/Uptake.java b/src/main/java/org/team157/robot/subsystems/uptake/Uptake.java index 44d5398..9365ed7 100644 --- a/src/main/java/org/team157/robot/subsystems/uptake/Uptake.java +++ b/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); diff --git a/src/main/java/org/team157/robot/subsystems/uptake/UptakeConstants.java b/src/main/java/org/team157/robot/subsystems/uptake/UptakeConstants.java index 58b0ca8..8c4e9d9 100644 --- a/src/main/java/org/team157/robot/subsystems/uptake/UptakeConstants.java +++ b/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; } diff --git a/src/main/java/org/team157/robot/subsystems/uptake/UptakeIO.java b/src/main/java/org/team157/robot/subsystems/uptake/UptakeIO.java index e0dff2c..3ec32a3 100644 --- a/src/main/java/org/team157/robot/subsystems/uptake/UptakeIO.java +++ b/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 dutyCycle) { + return Commands.none(); + } } diff --git a/src/main/java/org/team157/robot/subsystems/uptake/UptakeIOTalonFX.java b/src/main/java/org/team157/robot/subsystems/uptake/UptakeIOTalonFX.java index 76e72ef..c58ec8d 100644 --- a/src/main/java/org/team157/robot/subsystems/uptake/UptakeIOTalonFX.java +++ b/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 dutyCycle) { + return uptake.set(dutyCycle); + } + + @Override + public Command set(DoubleSupplier dutyCycle) { + return set((Supplier) dutyCycle::getAsDouble); + } + @Override public void simIterate() { uptake.simIterate();