From e9e9f692aad7b66cc6dd512a6ddf00951e63db5a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 12:42:41 -0700 Subject: [PATCH 1/6] (untested) decrease shot vel and such --- .../subsystems/intake/SlapdownSubsystem.java | 10 ++++++++-- .../java/frc/robot/utils/autoaim/AutoAim.java | 16 ++++++++-------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 0174bac5..efe41cc5 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -31,7 +31,7 @@ import org.littletonrobotics.junction.Logger; public class SlapdownSubsystem extends SubsystemBase implements Intake { - public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromRotations(-0.051025); + public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = Rotation2d.fromDegrees(122); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; @@ -160,7 +160,13 @@ public Command restExtended() { pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); rollerIO.setRollerVoltage(0.0); }) - .unless(atExtensionTrigger); + .until(atExtensionTrigger) + .andThen( + this.run( + () -> { + pivotIO.setMotorVoltage(0); + rollerIO.setRollerVoltage(0); + })); } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 8ec53e41..88e96f6e 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -56,34 +56,34 @@ public class AutoAim { static { // For hub shot tree // TODO min shot COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 2, 1.04)); + Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 4, 1.04)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(25), 35 - 2, 1.14)); + new ShotData(Rotation2d.fromDegrees(25), 35 - 4, 1.14)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), - new ShotData(Rotation2d.fromDegrees(26), 37 - 2, 1.10)); + new ShotData(Rotation2d.fromDegrees(26), 37 - 4, 1.10)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), - new ShotData(Rotation2d.fromDegrees(30), 37 - 2, 1.09)); + new ShotData(Rotation2d.fromDegrees(30), 37 - 4, 1.09)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), - new ShotData(Rotation2d.fromDegrees(33), 37 - 2, 1.15)); + new ShotData(Rotation2d.fromDegrees(33), 37 - 4, 1.15)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), - new ShotData(Rotation2d.fromDegrees(36), 38 - 2, 1.23)); + new ShotData(Rotation2d.fromDegrees(36), 38 - 4, 1.23)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), - new ShotData(Rotation2d.fromDegrees(38), 39 - 2, 1.33)); + new ShotData(Rotation2d.fromDegrees(38), 39 - 4, 1.33)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), - new ShotData(Rotation2d.fromDegrees(39), 40.5 - 1, 1.35)); + new ShotData(Rotation2d.fromDegrees(39), 40.5 - 4, 1.35)); } // Ig we'll see if we need more than 1 feed shot tree From c1d1a0a89c8b9b022cc0618918bef7f5123441cf Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 13:15:12 -0700 Subject: [PATCH 2/6] disable trench align --- src/main/java/frc/robot/Robot.java | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0964d1d..35ef5815 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -616,20 +616,20 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE)); - new Trigger(swerve::isNearTrench) - .and(() -> Superstructure.getState() != SuperState.INTAKE) - .and(() -> isTeleopEnabled()) - .and(() -> !Superstructure.getPoseOverride()) - .whileTrue( - swerve - .trenchAlign( - () -> - modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - () -> - modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()) - .alongWith(Commands.print("afkljsdflkjs"))); + // new Trigger(swerve::isNearTrench) + // .and(() -> Superstructure.getState() != SuperState.INTAKE) + // .and(() -> isTeleopEnabled()) + // .and(() -> !Superstructure.getPoseOverride()) + // .whileTrue( + // swerve + // .trenchAlign( + // () -> + // modifyJoystick(driver.getLeftY()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getLeftX()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()) + // .alongWith(Commands.print("afkljsdflkjs"))); // current zero shooter hood driver From 5d28b295a6578095193745ab73f424cc00944310 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 13:35:45 -0700 Subject: [PATCH 3/6] dsglkasjfdlkjs --- src/main/java/frc/robot/Robot.java | 34 +++++++++---------- src/main/java/frc/robot/Superstructure.java | 14 ++++---- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../subsystems/shooter/ShooterSubsystem.java | 3 +- .../subsystems/shooter/TurretSubsystem.java | 32 ++++++++++------- 5 files changed, 47 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 35ef5815..b5fbf471 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,7 +36,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Superstructure.SuperState; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.candle.CANdleIOReal; @@ -69,6 +68,7 @@ import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.ClimbTargets; +import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; import frc.robot.utils.autoaim.AutoAim; import java.util.Arrays; @@ -508,7 +508,7 @@ public Robot() { driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); shooter.setDefaultCommand( - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, superstructure::canScore)); + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, superstructure::inScoringArea, () -> FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose())); swerve.setDefaultCommand( swerve .driveOpenLoopFieldRelative( @@ -600,21 +600,21 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta : Rotation2d.k180deg))); // autoaim (alpha) - autoAimReq - .and(() -> ROBOT_EDITION == RobotEdition.ALPHA) - .whileTrue( - swerve.faceHub( - () -> - -1 - * modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - () -> - -1 - * modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)); + // autoAimReq + // .and(() -> ROBOT_EDITION == RobotEdition.ALPHA) + // .whileTrue( + // swerve.faceHub( + // () -> + // -1 + // * modifyJoystick(driver.getLeftY()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // () -> + // -1 + // * modifyJoystick(driver.getLeftX()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // ROBOT_EDITION == RobotEdition.ALPHA + // ? AutoAim.ALPHA_HUB_SHOT_TREE + // : AutoAim.COMP_HUB_SHOT_TREE)); // new Trigger(swerve::isNearTrench) // .and(() -> Superstructure.getState() != SuperState.INTAKE) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 7c60780f..8678c20d 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -197,8 +197,8 @@ private void addTriggers() { operator.leftBumper().onTrue(Commands.runOnce(() -> feedTarget = FeedTarget.LEFT)); operator.rightBumper().onTrue(Commands.runOnce(() -> feedTarget = FeedTarget.RIGHT)); - operator.leftTrigger().onTrue(Commands.runOnce(() -> poseOverride = true)); - operator.rightTrigger().onTrue(Commands.runOnce(() -> poseOverride = false)); + // operator.leftTrigger().onTrue(Commands.runOnce(() -> poseOverride = true)); + // operator.rightTrigger().onTrue(Commands.runOnce(() -> poseOverride = false)); operator .povLeft() @@ -384,14 +384,14 @@ private void addCommands() { intake.restExtended(), // intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( SuperState.INTAKE, intake.intake(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( @@ -565,19 +565,19 @@ private void addCommands() { SuperState.PRE_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); bindCommands( SuperState.CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( SuperState.POST_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d57152f7..45cbba4b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,7 +40,7 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore); + BooleanSupplier canScore, Supplier feedTarget); /** Run balls out from the shooter. This is for antijamming the robot */ public Command spit(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5f208e48..d80d603e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -119,7 +119,8 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore) { + BooleanSupplier canScore, + Supplier feedTarget) { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index e70fd515..1cf3fc87 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -27,7 +27,6 @@ import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; -import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.BooleanSupplier; @@ -204,24 +203,33 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore) { + BooleanSupplier inScoringArea, + Supplier feedTarget) { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ANGLE); flywheelIO.setFlywheelVoltage(0.0); - if (canScore.getAsBoolean()) { - turretIO.setTurretPosition( - AutoAim.getTurretFeedTargetRotation( - FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + // if (canScore.getAsBoolean()) { + // turretIO.setTurretPosition( + // AutoAim.getTurretFeedTargetRotation( + // FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation(), + // robotPoseSupplier.get(), + // chassisSpeedsSupplier.get())); + // } else { + if (inScoringArea.getAsBoolean()) { + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); } else { turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + AutoAim.getTurretFeedTargetRotation( + feedTarget.get().getTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); } + // } }); } From 5250a886e66b4fb734a331fb4df30b691b2094a7 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 14:09:52 -0700 Subject: [PATCH 4/6] i don't even know --- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/Superstructure.java | 30 +++++-- .../frc/robot/subsystems/shooter/Shooter.java | 3 +- .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../subsystems/shooter/TurretSubsystem.java | 84 ++++++++++--------- .../java/frc/robot/utils/autoaim/AutoAim.java | 16 ++-- 6 files changed, 84 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b5fbf471..9251c0cb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -508,7 +508,11 @@ public Robot() { driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); shooter.setDefaultCommand( - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, superstructure::inScoringArea, () -> FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose())); + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + superstructure::inScoringArea, + () -> FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose())); swerve.setDefaultCommand( swerve .driveOpenLoopFieldRelative( diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 8678c20d..fedf75e7 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -384,14 +384,22 @@ private void addCommands() { intake.restExtended(), // intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( SuperState.INTAKE, intake.intake(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( @@ -565,19 +573,31 @@ private void addCommands() { SuperState.PRE_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); bindCommands( SuperState.CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( SuperState.POST_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 45cbba4b..6b9f01a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,7 +40,8 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore, Supplier feedTarget); + BooleanSupplier canScore, + Supplier feedTarget); /** Run balls out from the shooter. This is for antijamming the robot */ public Command spit(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index d80d603e..054753b7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -119,7 +119,7 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore, + BooleanSupplier canScore, Supplier feedTarget) { return this.run( () -> { diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 1cf3fc87..5d190b81 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -217,17 +217,17 @@ public Command rest( // chassisSpeedsSupplier.get())); // } else { if (inScoringArea.getAsBoolean()) { - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); } else { turretIO.setTurretPosition( - AutoAim.getTurretFeedTargetRotation( - feedTarget.get().getTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + AutoAim.getTurretFeedTargetRotation( + feedTarget.get().getTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); } // } }); @@ -261,38 +261,40 @@ public Command score( Supplier robotPoseSupplier, Supplier shotDataSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - switch (Superstructure.getFixedShotTarget()) { - // in front of left trench with intake facing trench - case LEFT: - hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); - // in front of tower with intake facing left (to avoid deadzone) - case MID: - hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); - // in front of right trench with intake facing alliance wall - case RIGHT: - hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); - case NONE: - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); - } - }); + return resetTurretToCalculatedPosition() + .andThen( + this.run( + () -> { + switch (Superstructure.getFixedShotTarget()) { + // in front of left trench with intake facing trench + case LEFT: + hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); + // in front of tower with intake facing left (to avoid deadzone) + case MID: + hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); + // in front of right trench with intake facing alliance wall + case RIGHT: + hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); + case NONE: + hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + shotDataSupplier.get().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } + })); } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 86929f94..dc1c43c8 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -61,34 +61,34 @@ public class AutoAim { static { // For hub shot tree // TODO min shot COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 4, 1.04)); + Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6, 1.04)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(25), 35 - 4, 1.14)); + new ShotData(Rotation2d.fromDegrees(25), 35 - 6, 1.14)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), - new ShotData(Rotation2d.fromDegrees(26), 37 - 4, 1.10)); + new ShotData(Rotation2d.fromDegrees(26), 37 - 6, 1.10)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), - new ShotData(Rotation2d.fromDegrees(30), 37 - 4, 1.09)); + new ShotData(Rotation2d.fromDegrees(30), 37 - 6, 1.09)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), - new ShotData(Rotation2d.fromDegrees(33), 37 - 4, 1.15)); + new ShotData(Rotation2d.fromDegrees(33), 37 - 6, 1.15)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), - new ShotData(Rotation2d.fromDegrees(36), 38 - 4, 1.23)); + new ShotData(Rotation2d.fromDegrees(36), 38 - 6, 1.23)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), - new ShotData(Rotation2d.fromDegrees(38), 39 - 4, 1.33)); + new ShotData(Rotation2d.fromDegrees(38), 39 - 6, 1.33)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), - new ShotData(Rotation2d.fromDegrees(39), 40.5 - 4, 1.35)); + new ShotData(Rotation2d.fromDegrees(39), 40.5 - 6, 1.35)); } // Ig we'll see if we need more than 1 feed shot tree From 8fe76a649ce3a90ddf806ecb82753871fac18130 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 14:18:28 -0700 Subject: [PATCH 5/6] WE ARE SO BACK --- .../frc/robot/subsystems/indexer/SpindexerSubsystem.java | 6 +++--- .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index ffeeed31..c9ab9abe 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -71,10 +71,10 @@ public Command kick(DoubleSupplier flywheelSpeedSupplier) { double spinnerSpeed = surfaceSpeedInPerSec / (Math.PI * SPINNER_DIAMETER_INCHES); Logger.recordOutput("Indexer/Spinner/Adjusted speed", spinnerSpeed); Logger.recordOutput("Indexer/Kicker/Adjusted speed", kickerSpeed); - spinnerIO.setRollerVelocity(spinnerSpeed - 1); + // spinnerIO.setRollerVelocity(spinnerSpeed - 1); // kickerIO.setRollerVelocity(kickerSpeed - 5); - // spinnerIO.setRollerVelocity(20); - kickerIO.setRollerVelocity(15); + spinnerIO.setRollerVelocity(30); + kickerIO.setRollerVelocity(25); }) // .withTimeout(3), // this.run( diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 9d6aa8e3..eaf9622f 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -210,8 +210,8 @@ public static TalonFXConfiguration getPivotConfig() { config.CurrentLimits.SupplyCurrentLimitEnable = true; // TODO: TUNE - config.MotionMagic.MotionMagicCruiseVelocity = 1; - config.MotionMagic.MotionMagicAcceleration = 1; + config.MotionMagic.MotionMagicCruiseVelocity = 5; + config.MotionMagic.MotionMagicAcceleration = 5; return config; } From 634df9c626ae736da122d2f67266f65e584a3ee6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 13 Mar 2026 19:19:46 -0700 Subject: [PATCH 6/6] speculatively change feed shot vel and also just score auto and also fix depot climb to go to the correct climb target --- src/main/java/frc/robot/Autos.java | 6 +- src/main/java/frc/robot/Robot.java | 1 + .../indexer/SpindexerSubsystem.java | 2 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 67 ++++++++++++------- 4 files changed, 51 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 433e649c..0d976eee 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -505,7 +505,7 @@ public Command getDepotClimbAuto() { lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1); Path[] paths = {Path.PLtoD, Path.DtoCL}; Command autoCommand = - paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse()); + paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoTrue()); for (Path p : paths) { autoCommand = autoCommand.andThen(runPath(p, routine)); @@ -548,4 +548,8 @@ public Command getTestAuto() { public Command waitUntilEmpty() { return Commands.waitSeconds(3.0); } + + public Command getJustScoreAuto() { + return setAutoScoreReqTrue().andThen(Commands.waitSeconds(5)).andThen(setAutoScoreReqFalse()); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9251c0cb..f5c3cfde 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -726,6 +726,7 @@ private void addAutos() { autoChooser.addOption("Depot Climb", autos.getDepotClimbAuto()); autoChooser.addOption("Depot Outpost Climb", autos.getDepotOutpostClimbAuto()); autoChooser.addOption("Test Auto", autos.getTestAuto()); + autoChooser.addOption("Just Score", autos.getJustScoreAuto()); haveAutosGenerated = true; System.out.println("Done generating autos"); diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index c9ab9abe..f9442e60 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -73,7 +73,7 @@ public Command kick(DoubleSupplier flywheelSpeedSupplier) { Logger.recordOutput("Indexer/Kicker/Adjusted speed", kickerSpeed); // spinnerIO.setRollerVelocity(spinnerSpeed - 1); // kickerIO.setRollerVelocity(kickerSpeed - 5); - spinnerIO.setRollerVelocity(30); + spinnerIO.setRollerVelocity(60); kickerIO.setRollerVelocity(25); }) // .withTimeout(3), diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index dc1c43c8..6a12c504 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -95,29 +95,50 @@ public class AutoAim { public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); static { // For feed shot tree - FEED_SHOT_TREE.put(Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20, 0)); - FEED_SHOT_TREE.put(Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40, 0.0)); - - FEED_SHOT_TREE.put(Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38, 1.42)); - FEED_SHOT_TREE.put(Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40, 1.36)); - FEED_SHOT_TREE.put(Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41, 1.34)); - FEED_SHOT_TREE.put(Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42, 1.25)); - FEED_SHOT_TREE.put(Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43, 1.28)); - FEED_SHOT_TREE.put(Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45, 1.27)); - FEED_SHOT_TREE.put(Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46, 1.32)); - FEED_SHOT_TREE.put(Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48, 1.4)); - - FEED_SHOT_TREE.put(Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49, 1.3)); - FEED_SHOT_TREE.put(Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53, 1.33)); - FEED_SHOT_TREE.put(Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57, 1.3)); - FEED_SHOT_TREE.put(Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57, 1.2)); - FEED_SHOT_TREE.put(Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59, 1.2)); + FEED_SHOT_TREE.put( + Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); + + FEED_SHOT_TREE.put( + Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42)); + FEED_SHOT_TREE.put( + Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36)); + FEED_SHOT_TREE.put( + Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34)); + FEED_SHOT_TREE.put( + Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25)); + FEED_SHOT_TREE.put( + Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28)); + FEED_SHOT_TREE.put( + Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27)); + FEED_SHOT_TREE.put( + Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32)); + FEED_SHOT_TREE.put( + Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4)); + + FEED_SHOT_TREE.put( + Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3)); + FEED_SHOT_TREE.put( + Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33)); + FEED_SHOT_TREE.put( + Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3)); + FEED_SHOT_TREE.put( + Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2)); + FEED_SHOT_TREE.put( + Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2)); // TODO: POPULATE beyond 24 feet and time of flight }