From 43d20edaf28def157b80189d0ff7ca3e3debf1e6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 14:38:41 -0700 Subject: [PATCH 1/8] retune shots yay --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/Superstructure.java | 38 ++++++++++--------- .../frc/robot/subsystems/shooter/Shooter.java | 5 +++ .../subsystems/shooter/TurretSubsystem.java | 19 ++++++++++ .../java/frc/robot/utils/autoaim/AutoAim.java | 28 +++++++++++++- 5 files changed, 71 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9eb09008..5de4df7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -149,7 +149,7 @@ public enum RobotEdition { * This is for when we're testing shot and extension numbers and should be FALSE once bring up is * complete */ - public static final boolean TUNING_MODE = false; + public static final boolean TUNING_MODE = true; public boolean hasZeroedSinceStartup = false; diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f9fdb6b4..9c51101a 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -530,16 +530,17 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.restExtended(), indexer.rest(), - shooter.score( + // shooter.score( + shooter.testShot( swerve::getPose, - () -> - AutoAim.getCompensatedSOTMShotData( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), + // () -> + // AutoAim.getCompensatedSOTMShotData( + // shooter.getTurretPose(swerve.getPose()), + // FieldUtils.getCurrentHubTranslation(), + // swerve.getVelocityFieldRelative(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? AutoAim.ALPHA_HUB_SHOT_TREE + // : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); @@ -557,16 +558,17 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), - shooter.score( + // shooter.score( + shooter.testShot( swerve::getPose, - () -> - AutoAim.getCompensatedSOTMShotData( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), + // () -> + // AutoAim.getCompensatedSOTMShotData( + // shooter.getTurretPose(swerve.getPose()), + // FieldUtils.getCurrentHubTranslation(), + // swerve.getVelocityFieldRelative(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? AutoAim.ALPHA_HUB_SHOT_TREE + // : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6b9f01a4..479af718 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -96,4 +96,9 @@ public default Command currentZeroTurretAgainstForwardHardstop() { public default Command stopTurret() { return Commands.none(); } + + public default Command testShot( + Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index b873e0e6..1d50a444 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -26,6 +26,7 @@ import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; +import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.BooleanSupplier; @@ -113,6 +114,10 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { "Turret may have gone past hardstop!! Reoffset cancoders + min/max position", AlertType.kError); + private LoggedTunableNumber testDegrees = + new LoggedTunableNumber("Shooter/Test Hood Degrees", 50.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 50.0); + public TurretSubsystem( FlywheelIO flywheelIO, HoodIO hoodIO, @@ -198,6 +203,20 @@ public Command feed( }); } + public Command testShot( + Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + }); + } + @Override public Command rest( Supplier robotPoseSupplier, diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index e461d583..7984d91b 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -58,6 +58,30 @@ public class AutoAim { public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree(); + static { + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 24), + new ShotData(Rotation2d.fromDegrees(25), 32, 0.81)); + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 2 * 24), + new ShotData(Rotation2d.fromDegrees(27), 34, 1.09)); + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 3 * 24), + new ShotData(Rotation2d.fromDegrees(29), 36, 1.1)); + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 4 * 24), + new ShotData(Rotation2d.fromDegrees(31), 40, 1.3)); + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 5 * 24), + new ShotData(Rotation2d.fromDegrees(33), 42, 1.02)); + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 * 24), + new ShotData(Rotation2d.fromDegrees(35), 43, 1.3)); + COMP_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 7 * 24), + new ShotData(Rotation2d.fromDegrees(37), 47, 1.46)); + } + // TODO update tof static { // For hub shot tree // TODO min shot @@ -67,7 +91,7 @@ public class AutoAim { COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(25), 35 - 6 + 3, 1.14)); + new ShotData(Rotation2d.fromDegrees(25), 35 - 6 + 3, 0.9)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), @@ -76,7 +100,7 @@ public class AutoAim { 37 - 6 + 3 // - 6 , - 1.10)); + 1.02)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), From 9db8a8597908648306a435a8a9a5f2aa16a2a8ad Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 14:39:15 -0700 Subject: [PATCH 2/8] turn off test shots --- src/main/java/frc/robot/Superstructure.java | 40 ++++++++++----------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9c51101a..5e8c4cd0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -530,17 +530,17 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.restExtended(), indexer.rest(), - // shooter.score( - shooter.testShot( + shooter.score( + // shooter.testShot( swerve::getPose, - // () -> - // AutoAim.getCompensatedSOTMShotData( - // shooter.getTurretPose(swerve.getPose()), - // FieldUtils.getCurrentHubTranslation(), - // swerve.getVelocityFieldRelative(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? AutoAim.ALPHA_HUB_SHOT_TREE - // : AutoAim.COMP_HUB_SHOT_TREE), + () -> + AutoAim.getCompensatedSOTMShotData( + shooter.getTurretPose(swerve.getPose()), + FieldUtils.getCurrentHubTranslation(), + swerve.getVelocityFieldRelative(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); @@ -558,17 +558,17 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), - // shooter.score( - shooter.testShot( + shooter.score( + // shooter.testShot( swerve::getPose, - // () -> - // AutoAim.getCompensatedSOTMShotData( - // shooter.getTurretPose(swerve.getPose()), - // FieldUtils.getCurrentHubTranslation(), - // swerve.getVelocityFieldRelative(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? AutoAim.ALPHA_HUB_SHOT_TREE - // : AutoAim.COMP_HUB_SHOT_TREE), + () -> + AutoAim.getCompensatedSOTMShotData( + shooter.getTurretPose(swerve.getPose()), + FieldUtils.getCurrentHubTranslation(), + swerve.getVelocityFieldRelative(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); From 149b9d782b93218cb853da633458f83d7ac417f1 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 14:44:49 -0700 Subject: [PATCH 3/8] fmt? --- src/main/java/frc/robot/Superstructure.java | 4 ++-- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 5e8c4cd0..0eb18005 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -531,7 +531,7 @@ private void addCommands() { intake.restExtended(), indexer.rest(), shooter.score( - // shooter.testShot( + // shooter.testShot( swerve::getPose, () -> AutoAim.getCompensatedSOTMShotData( @@ -559,7 +559,7 @@ private void addCommands() { : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.score( - // shooter.testShot( + // shooter.testShot( swerve::getPose, () -> AutoAim.getCompensatedSOTMShotData( diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 7984d91b..d5a425d4 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -62,22 +62,22 @@ public class AutoAim { COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 24), new ShotData(Rotation2d.fromDegrees(25), 32, 0.81)); - COMP_HUB_SHOT_TREE.put( + COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 2 * 24), new ShotData(Rotation2d.fromDegrees(27), 34, 1.09)); - COMP_HUB_SHOT_TREE.put( + COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 3 * 24), new ShotData(Rotation2d.fromDegrees(29), 36, 1.1)); - COMP_HUB_SHOT_TREE.put( + COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 4 * 24), new ShotData(Rotation2d.fromDegrees(31), 40, 1.3)); - COMP_HUB_SHOT_TREE.put( + COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 5 * 24), new ShotData(Rotation2d.fromDegrees(33), 42, 1.02)); - COMP_HUB_SHOT_TREE.put( + COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 * 24), new ShotData(Rotation2d.fromDegrees(35), 43, 1.3)); - COMP_HUB_SHOT_TREE.put( + COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 7 * 24), new ShotData(Rotation2d.fromDegrees(37), 47, 1.46)); } From f523e97c79edda453b036d122e6a2ee38ede0bf2 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 19:54:35 -0700 Subject: [PATCH 4/8] retune shots again --- src/main/java/frc/robot/Robot.java | 8 ++ src/main/java/frc/robot/Superstructure.java | 40 ++++---- .../subsystems/shooter/TurretSubsystem.java | 2 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 94 ++++--------------- 4 files changed, 49 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5de4df7b..7d6d31e0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -67,6 +67,7 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.FieldUtils; import frc.robot.utils.FieldUtils.ClimbTargets; import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; @@ -849,6 +850,13 @@ public void robotPeriodic() { Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone()); noLogStickAlert.set(!directory.exists()); + + Logger.recordOutput( + "Distance to hub", + shooter + .getTurretPose(swerve.getPose()) + .getTranslation() + .getDistance(FieldUtils.getCurrentHubTranslation())); } public void updateAlerts() { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 0eb18005..9c51101a 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -530,17 +530,17 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.restExtended(), indexer.rest(), - shooter.score( - // shooter.testShot( + // shooter.score( + shooter.testShot( swerve::getPose, - () -> - AutoAim.getCompensatedSOTMShotData( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), + // () -> + // AutoAim.getCompensatedSOTMShotData( + // shooter.getTurretPose(swerve.getPose()), + // FieldUtils.getCurrentHubTranslation(), + // swerve.getVelocityFieldRelative(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? AutoAim.ALPHA_HUB_SHOT_TREE + // : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); @@ -558,17 +558,17 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), - shooter.score( - // shooter.testShot( + // shooter.score( + shooter.testShot( swerve::getPose, - () -> - AutoAim.getCompensatedSOTMShotData( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), + // () -> + // AutoAim.getCompensatedSOTMShotData( + // shooter.getTurretPose(swerve.getPose()), + // FieldUtils.getCurrentHubTranslation(), + // swerve.getVelocityFieldRelative(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? AutoAim.ALPHA_HUB_SHOT_TREE + // : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 1d50a444..908db2fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -468,7 +468,7 @@ public static TalonFXConfiguration getFlywheelConfig() { config.Slot0.kS = 0.79522; // 0.63933; config.Slot0.kV = 0.11087; // 0.11582; config.Slot0.kA = 0.026101; // 0.020809; - config.Slot0.kP = 0.6; + config.Slot0.kP = 0.4; config.Slot0.kD = 0; // slot 1 is for torque current diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index d5a425d4..4c95b3db 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -1,5 +1,7 @@ package frc.robot.utils.autoaim; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -11,7 +13,6 @@ import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; -import org.littletonrobotics.junction.Logger; public class AutoAim { @@ -58,6 +59,24 @@ public class AutoAim { public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree(); + static { + COMP_HUB_SHOT_TREE.put(1.716849, new ShotData(Rotation2d.fromDegrees(23), 30, 0.8)); + COMP_HUB_SHOT_TREE.put(2.017596, new ShotData(Rotation2d.fromDegrees(23), 33, 0.9)); + COMP_HUB_SHOT_TREE.put(2.423868, new ShotData(Rotation2d.fromDegrees(25), 35, 1.1)); + COMP_HUB_SHOT_TREE.put(2.664198, new ShotData(Rotation2d.fromDegrees(26), 36, 1.2)); + COMP_HUB_SHOT_TREE.put(2.903207, new ShotData(Rotation2d.fromDegrees(30), 35, 1.2)); + COMP_HUB_SHOT_TREE.put(3.156802, new ShotData(Rotation2d.fromDegrees(32), 35, 1.23)); + COMP_HUB_SHOT_TREE.put(3.437033, new ShotData(Rotation2d.fromDegrees(34), 35, 1.25)); + COMP_HUB_SHOT_TREE.put(3.611052, new ShotData(Rotation2d.fromDegrees(38), 34, 1.24)); + COMP_HUB_SHOT_TREE.put(3.773999, new ShotData(Rotation2d.fromDegrees(39), 34, 1.21)); + COMP_HUB_SHOT_TREE.put(3.899275, new ShotData(Rotation2d.fromDegrees(40), 34, 1.2)); + COMP_HUB_SHOT_TREE.put(4.138058, new ShotData(Rotation2d.fromDegrees(41), 34, 1.13)); + COMP_HUB_SHOT_TREE.put(4.602258, new ShotData(Rotation2d.fromDegrees(43), 34.5, 1.15)); + COMP_HUB_SHOT_TREE.put(4.893493, new ShotData(Rotation2d.fromDegrees(45), 35, 1.2)); + COMP_HUB_SHOT_TREE.put(5.225402, new ShotData(Rotation2d.fromDegrees(47), 35, 1.2)); + COMP_HUB_SHOT_TREE.put(5.584793, new ShotData(Rotation2d.fromDegrees(49), 35.5, 1.17)); + } + static { COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 24), @@ -82,79 +101,6 @@ public class AutoAim { new ShotData(Rotation2d.fromDegrees(37), 47, 1.46)); } - // TODO update tof - static { // For hub shot tree - // TODO min shot - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 + 17), - new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6 + 3, 1.04)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(25), 35 - 6 + 3, 0.9)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), - new ShotData( - Rotation2d.fromDegrees(26), - 37 - 6 + 3 - // - 6 - , - 1.02)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), - new ShotData( - Rotation2d.fromDegrees(30), - 37 - 6 + 3 - // - 6 - , - 1.09)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), - new ShotData( - Rotation2d.fromDegrees(33), - 37 - 6 + 3 - // - 6 - , - 1.15)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), - new ShotData( - Rotation2d.fromDegrees(36), - 38 - 6 + 3 - // - 6 - , - 1.23)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), - new ShotData( - Rotation2d.fromDegrees(38), - 39 - 6 + 3 - // - 6 - , - 1.33)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), - new ShotData( - Rotation2d.fromDegrees(39), - 40.5 - 6 + 3 - // - 6 - , - 1.35)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12 + 6), - new ShotData( - Rotation2d.fromDegrees(39), - 40.5 - 6 + 3 + 2 - // - 6 - , - 1.35)); - } - // Ig we'll see if we need more than 1 feed shot tree public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); From 8ad1e6623ce71ab07b5f60f728390e0172ff9fd9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 22:44:20 -0700 Subject: [PATCH 5/8] fmt i think --- src/main/java/frc/robot/Superstructure.java | 40 ++++++++--------- .../java/frc/robot/utils/autoaim/AutoAim.java | 45 +++++-------------- 2 files changed, 30 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9c51101a..0eb18005 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -530,17 +530,17 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.restExtended(), indexer.rest(), - // shooter.score( - shooter.testShot( + shooter.score( + // shooter.testShot( swerve::getPose, - // () -> - // AutoAim.getCompensatedSOTMShotData( - // shooter.getTurretPose(swerve.getPose()), - // FieldUtils.getCurrentHubTranslation(), - // swerve.getVelocityFieldRelative(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? AutoAim.ALPHA_HUB_SHOT_TREE - // : AutoAim.COMP_HUB_SHOT_TREE), + () -> + AutoAim.getCompensatedSOTMShotData( + shooter.getTurretPose(swerve.getPose()), + FieldUtils.getCurrentHubTranslation(), + swerve.getVelocityFieldRelative(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); @@ -558,17 +558,17 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), - // shooter.score( - shooter.testShot( + shooter.score( + // shooter.testShot( swerve::getPose, - // () -> - // AutoAim.getCompensatedSOTMShotData( - // shooter.getTurretPose(swerve.getPose()), - // FieldUtils.getCurrentHubTranslation(), - // swerve.getVelocityFieldRelative(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? AutoAim.ALPHA_HUB_SHOT_TREE - // : AutoAim.COMP_HUB_SHOT_TREE), + () -> + AutoAim.getCompensatedSOTMShotData( + shooter.getTurretPose(swerve.getPose()), + FieldUtils.getCurrentHubTranslation(), + swerve.getVelocityFieldRelative(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), climber.retract()); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 4c95b3db..658de9ce 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -1,7 +1,5 @@ package frc.robot.utils.autoaim; -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,6 +11,7 @@ import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import org.littletonrobotics.junction.Logger; public class AutoAim { @@ -59,15 +58,15 @@ public class AutoAim { public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree(); - static { - COMP_HUB_SHOT_TREE.put(1.716849, new ShotData(Rotation2d.fromDegrees(23), 30, 0.8)); - COMP_HUB_SHOT_TREE.put(2.017596, new ShotData(Rotation2d.fromDegrees(23), 33, 0.9)); - COMP_HUB_SHOT_TREE.put(2.423868, new ShotData(Rotation2d.fromDegrees(25), 35, 1.1)); - COMP_HUB_SHOT_TREE.put(2.664198, new ShotData(Rotation2d.fromDegrees(26), 36, 1.2)); - COMP_HUB_SHOT_TREE.put(2.903207, new ShotData(Rotation2d.fromDegrees(30), 35, 1.2)); - COMP_HUB_SHOT_TREE.put(3.156802, new ShotData(Rotation2d.fromDegrees(32), 35, 1.23)); - COMP_HUB_SHOT_TREE.put(3.437033, new ShotData(Rotation2d.fromDegrees(34), 35, 1.25)); - COMP_HUB_SHOT_TREE.put(3.611052, new ShotData(Rotation2d.fromDegrees(38), 34, 1.24)); + static { + COMP_HUB_SHOT_TREE.put(1.716849, new ShotData(Rotation2d.fromDegrees(23), 30, 0.8)); + COMP_HUB_SHOT_TREE.put(2.017596, new ShotData(Rotation2d.fromDegrees(23), 33, 0.9)); + COMP_HUB_SHOT_TREE.put(2.423868, new ShotData(Rotation2d.fromDegrees(25), 35, 1.1)); + COMP_HUB_SHOT_TREE.put(2.664198, new ShotData(Rotation2d.fromDegrees(26), 36, 1.2)); + COMP_HUB_SHOT_TREE.put(2.903207, new ShotData(Rotation2d.fromDegrees(30), 35, 1.2)); + COMP_HUB_SHOT_TREE.put(3.156802, new ShotData(Rotation2d.fromDegrees(32), 35, 1.23)); + COMP_HUB_SHOT_TREE.put(3.437033, new ShotData(Rotation2d.fromDegrees(34), 35, 1.25)); + COMP_HUB_SHOT_TREE.put(3.611052, new ShotData(Rotation2d.fromDegrees(38), 34, 1.24)); COMP_HUB_SHOT_TREE.put(3.773999, new ShotData(Rotation2d.fromDegrees(39), 34, 1.21)); COMP_HUB_SHOT_TREE.put(3.899275, new ShotData(Rotation2d.fromDegrees(40), 34, 1.2)); COMP_HUB_SHOT_TREE.put(4.138058, new ShotData(Rotation2d.fromDegrees(41), 34, 1.13)); @@ -75,30 +74,6 @@ public class AutoAim { COMP_HUB_SHOT_TREE.put(4.893493, new ShotData(Rotation2d.fromDegrees(45), 35, 1.2)); COMP_HUB_SHOT_TREE.put(5.225402, new ShotData(Rotation2d.fromDegrees(47), 35, 1.2)); COMP_HUB_SHOT_TREE.put(5.584793, new ShotData(Rotation2d.fromDegrees(49), 35.5, 1.17)); - } - - static { - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 24), - new ShotData(Rotation2d.fromDegrees(25), 32, 0.81)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 2 * 24), - new ShotData(Rotation2d.fromDegrees(27), 34, 1.09)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 3 * 24), - new ShotData(Rotation2d.fromDegrees(29), 36, 1.1)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 4 * 24), - new ShotData(Rotation2d.fromDegrees(31), 40, 1.3)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 5 * 24), - new ShotData(Rotation2d.fromDegrees(33), 42, 1.02)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 * 24), - new ShotData(Rotation2d.fromDegrees(35), 43, 1.3)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 7 * 24), - new ShotData(Rotation2d.fromDegrees(37), 47, 1.46)); } // Ig we'll see if we need more than 1 feed shot tree From 0b7a21c63e7b18805c066354fe3b98e378e4a9a1 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 22 Mar 2026 11:49:03 -0700 Subject: [PATCH 6/8] update vendordeps --- vendordeps/AdvantageKit.json | 6 +-- vendordeps/ChoreoLib2026.json | 6 +-- vendordeps/Phoenix6-frc2026-latest.json | 60 ++++++++++++------------- vendordeps/photonlib.json | 12 ++--- 4 files changed, 42 insertions(+), 42 deletions(-) diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 2faa4db8..469ae542 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "26.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "26.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "26.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json index cf7e7cf3..493c4ad8 100644 --- a/vendordeps/ChoreoLib2026.json +++ b/vendordeps/ChoreoLib2026.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2026.json", "name": "ChoreoLib", - "version": "2026.0.1", + "version": "2026.0.2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2026.0.1" + "version": "2026.0.2" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-frc2026-latest.json index 328a963c..056f5fbc 100644 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ b/vendordeps/Phoenix6-frc2026-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2026-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.2", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.2", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 548bb667..529f17b5 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1", + "version": "v2026.3.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2026.3.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.0.1-beta", + "version": "v2026.3.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2026.3.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.0.1-beta" + "version": "v2026.3.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.0.1-beta" + "version": "v2026.3.2" } ] } From efc07480d8dfca86267ffad286e00ab5a6c0ccdb Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 22 Mar 2026 11:58:42 -0700 Subject: [PATCH 7/8] fix gear ratios and redo ff --- src/main/java/frc/robot/Robot.java | 3 + .../frc/robot/subsystems/shooter/Shooter.java | 8 ++ .../subsystems/shooter/TurretSubsystem.java | 76 +++++++++++-- .../java/frc/robot/utils/autoaim/AutoAim.java | 105 +++++++++++++++--- 4 files changed, 170 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7d6d31e0..a4cc709c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -763,6 +763,9 @@ private void addAutos() { autoChooser.addOption("Right Bump Outpost Climb", autos.getRightBumpOutpostClimbAuto()); autoChooser.addOption("Right Bump Outpost Center", autos.getRightBumpOutpostCenterAuto()); + autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); + haveAutosGenerated = true; System.out.println("Done generating autos"); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 479af718..42bbcea8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -101,4 +101,12 @@ public default Command testShot( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { return Commands.none(); } + + public default Command runFlywheelSysid() { + return Commands.none(); + } + + public default Command runHoodSysid() { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 908db2fc..a5506166 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Volts; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -23,6 +25,10 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; @@ -40,10 +46,10 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ public static final double HOOD_GEAR_RATIO = 33.8671875; // 58.96875; - public static final double FLYWHEEL_GEAR_RATIO = 0.84615384615; + public static final double FLYWHEEL_GEAR_RATIO = 20.0 / 18.0; // 0.84615384615; - public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(73); - public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(23.16); + public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(56); + public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(11.33); public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0; public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find @@ -97,6 +103,24 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { private LinearFilter currentFilter = LinearFilter.movingAverage(10); + private SysIdRoutine hoodSysid = + new SysIdRoutine( + new Config( + null, + Volts.of(5), + null, + (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), + new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this)); + + private SysIdRoutine flywheelSysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), + new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); + private static final Alert cancoder24tDisconnectedAlert = new Alert("24T Cancoder disconnected!", AlertType.kError); private static final Alert cancoder26tDisconnectedAlert = @@ -465,9 +489,9 @@ public static TalonFXConfiguration getFlywheelConfig() { config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO; // slot 0 is for motion profiled velocity - config.Slot0.kS = 0.79522; // 0.63933; - config.Slot0.kV = 0.11087; // 0.11582; - config.Slot0.kA = 0.026101; // 0.020809; + config.Slot0.kS = 0.33706; // 0.63933; + config.Slot0.kV = 0.13893; // 0.11582; + config.Slot0.kA = 0.030026; // 0.020809; config.Slot0.kP = 0.4; config.Slot0.kD = 0; @@ -499,7 +523,7 @@ public static TalonFXConfiguration getHoodConfig() { config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.kS = 0.57613; - config.Slot0.kG = 0.35748; + config.Slot0.kG = 0.55748; config.Slot0.kV = 5.4081; config.Slot0.kA = 0.14829; config.Slot0.kP = 260.0; @@ -557,4 +581,42 @@ public static CANcoderConfiguration getCancoder26tConfigs() { return config; } + + @Override + public Command runHoodSysid() { + return Commands.sequence( + hoodSysid + .quasistatic(Direction.kForward) + .until( + () -> + hoodInputs.hoodPositionRotations.getDegrees() + > (HOOD_MAX_ANGLE.getDegrees() - 5)), // Stop before endstop + hoodSysid + .quasistatic(Direction.kReverse) + .until( + () -> + hoodInputs.hoodPositionRotations.getDegrees() + < (HOOD_MIN_ANGLE.getDegrees() + 5)), + hoodSysid + .dynamic(Direction.kForward) + .until( + () -> + hoodInputs.hoodPositionRotations.getDegrees() + > (HOOD_MAX_ANGLE.getDegrees() - 5)), + hoodSysid + .dynamic(Direction.kReverse) + .until( + () -> + hoodInputs.hoodPositionRotations.getDegrees() + < (HOOD_MIN_ANGLE.getDegrees() + 5))); + } + + @Override + public Command runFlywheelSysid() { + return Commands.sequence( + flywheelSysid.quasistatic(Direction.kForward), + flywheelSysid.quasistatic(Direction.kReverse), + flywheelSysid.dynamic(Direction.kForward), + flywheelSysid.dynamic(Direction.kReverse)); + } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 658de9ce..20bc2874 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -59,21 +59,96 @@ public class AutoAim { public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree(); static { - COMP_HUB_SHOT_TREE.put(1.716849, new ShotData(Rotation2d.fromDegrees(23), 30, 0.8)); - COMP_HUB_SHOT_TREE.put(2.017596, new ShotData(Rotation2d.fromDegrees(23), 33, 0.9)); - COMP_HUB_SHOT_TREE.put(2.423868, new ShotData(Rotation2d.fromDegrees(25), 35, 1.1)); - COMP_HUB_SHOT_TREE.put(2.664198, new ShotData(Rotation2d.fromDegrees(26), 36, 1.2)); - COMP_HUB_SHOT_TREE.put(2.903207, new ShotData(Rotation2d.fromDegrees(30), 35, 1.2)); - COMP_HUB_SHOT_TREE.put(3.156802, new ShotData(Rotation2d.fromDegrees(32), 35, 1.23)); - COMP_HUB_SHOT_TREE.put(3.437033, new ShotData(Rotation2d.fromDegrees(34), 35, 1.25)); - COMP_HUB_SHOT_TREE.put(3.611052, new ShotData(Rotation2d.fromDegrees(38), 34, 1.24)); - COMP_HUB_SHOT_TREE.put(3.773999, new ShotData(Rotation2d.fromDegrees(39), 34, 1.21)); - COMP_HUB_SHOT_TREE.put(3.899275, new ShotData(Rotation2d.fromDegrees(40), 34, 1.2)); - COMP_HUB_SHOT_TREE.put(4.138058, new ShotData(Rotation2d.fromDegrees(41), 34, 1.13)); - COMP_HUB_SHOT_TREE.put(4.602258, new ShotData(Rotation2d.fromDegrees(43), 34.5, 1.15)); - COMP_HUB_SHOT_TREE.put(4.893493, new ShotData(Rotation2d.fromDegrees(45), 35, 1.2)); - COMP_HUB_SHOT_TREE.put(5.225402, new ShotData(Rotation2d.fromDegrees(47), 35, 1.2)); - COMP_HUB_SHOT_TREE.put(5.584793, new ShotData(Rotation2d.fromDegrees(49), 35.5, 1.17)); + COMP_HUB_SHOT_TREE.put( + 1.716849, + new ShotData( + Rotation2d.fromDegrees(23 - 13.16), + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 0.8)); + COMP_HUB_SHOT_TREE.put( + 2.017596, + new ShotData( + Rotation2d.fromDegrees(23 - 13.16), + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 0.9)); + COMP_HUB_SHOT_TREE.put( + 2.423868, + new ShotData( + Rotation2d.fromDegrees(25 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.1)); + COMP_HUB_SHOT_TREE.put( + 2.664198, + new ShotData( + Rotation2d.fromDegrees(26 - 13.16), + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 2.903207, + new ShotData( + Rotation2d.fromDegrees(30 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 3.156802, + new ShotData( + Rotation2d.fromDegrees(32 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.23)); + COMP_HUB_SHOT_TREE.put( + 3.437033, + new ShotData( + Rotation2d.fromDegrees(34 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.25)); + COMP_HUB_SHOT_TREE.put( + 3.611052, + new ShotData( + Rotation2d.fromDegrees(38 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.24)); + COMP_HUB_SHOT_TREE.put( + 3.773999, + new ShotData( + Rotation2d.fromDegrees(39 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.21)); + COMP_HUB_SHOT_TREE.put( + 3.899275, + new ShotData( + Rotation2d.fromDegrees(40 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 4.138058, + new ShotData( + Rotation2d.fromDegrees(41 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.13)); + COMP_HUB_SHOT_TREE.put( + 4.602258, + new ShotData( + Rotation2d.fromDegrees(43 - 13.16), + 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.15)); + COMP_HUB_SHOT_TREE.put( + 4.893493, + new ShotData( + Rotation2d.fromDegrees(45 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 5.225402, + new ShotData( + Rotation2d.fromDegrees(47 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 5.584793, + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 1.17)); } // Ig we'll see if we need more than 1 feed shot tree From e59f3c07bd974dafcbceddfe0607c9dfab46b2b3 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 22 Mar 2026 12:06:31 -0700 Subject: [PATCH 8/8] speed up flywheels a smidge --- .../java/frc/robot/utils/autoaim/AutoAim.java | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 20bc2874..299ba2b0 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -63,91 +63,91 @@ public class AutoAim { 1.716849, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 0.8)); COMP_HUB_SHOT_TREE.put( 2.017596, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 0.9)); COMP_HUB_SHOT_TREE.put( 2.423868, new ShotData( Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.1)); COMP_HUB_SHOT_TREE.put( 2.664198, new ShotData( Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 2.903207, new ShotData( Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 3.156802, new ShotData( Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.23)); COMP_HUB_SHOT_TREE.put( 3.437033, new ShotData( Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.25)); COMP_HUB_SHOT_TREE.put( 3.611052, new ShotData( Rotation2d.fromDegrees(38 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.24)); COMP_HUB_SHOT_TREE.put( 3.773999, new ShotData( Rotation2d.fromDegrees(39 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.21)); COMP_HUB_SHOT_TREE.put( 3.899275, new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 4.138058, new ShotData( Rotation2d.fromDegrees(41 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.13)); COMP_HUB_SHOT_TREE.put( 4.602258, new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.15)); COMP_HUB_SHOT_TREE.put( 4.893493, new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 5.225402, new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 5.584793, new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, + 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.17)); }