From 2856b710bc339c4843ba7095f405e5336650cdf0 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 8 Mar 2026 17:05:03 +0000 Subject: [PATCH 1/2] Initial plan From 03ce695959d357cdd9bc8a2e9126b68757884495 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 8 Mar 2026 17:13:21 +0000 Subject: [PATCH 2/2] Improve ShotCalculator ease of use: tunable maps, drive-to-distance method, tuning auto routines Co-authored-by: GrumpyBud <109627078+GrumpyBud@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 24 ++++++ .../subsystems/shooter/ShotCalculator.java | 77 ++++++++++++++----- 2 files changed, 81 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bba1049..22c0b39 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.commands.DriveToPose; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Choreographer; import frc.robot.subsystems.drive.Drive; @@ -232,6 +233,29 @@ public RobotContainer() { autoChooser.addOption( "Test: Kicker", kicker.runCommand().withTimeout(3.0).withName("Test Kicker")); + // Tuning: drive to a fixed shot distance and hold — useful for quickly verifying map values. + // Robot positions itself directly in front of the hub at each preset distance. + autoChooser.addOption( + "Tune: Drive to Hub Distance (" + + ShotCalculator.hubPresetDistance + + "m)", + new DriveToPose(drive, () -> ShotCalculator.getAimedPoseAtDistance(ShotCalculator.hubPresetDistance))); + autoChooser.addOption( + "Tune: Drive to Tower Distance (" + + ShotCalculator.towerPresetDistance + + "m)", + new DriveToPose(drive, () -> ShotCalculator.getAimedPoseAtDistance(ShotCalculator.towerPresetDistance))); + autoChooser.addOption( + "Tune: Drive to Trench Distance (" + + ShotCalculator.trenchPresetDistance + + "m)", + new DriveToPose(drive, () -> ShotCalculator.getAimedPoseAtDistance(ShotCalculator.trenchPresetDistance))); + autoChooser.addOption( + "Tune: Drive to Outpost Distance (" + + ShotCalculator.outpostPresetDistance + + "m)", + new DriveToPose(drive, () -> ShotCalculator.getAimedPoseAtDistance(ShotCalculator.outpostPresetDistance))); + // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 039676a..49122f0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -133,6 +133,19 @@ public record ShootingParameters( public static record LaunchPreset( LoggedTunableNumber hoodAngleDeg, LoggedTunableNumber flywheelSpeed) {} + // Shooting map data — distances (m), default hood angles (deg), flywheel speeds (RPS), and + // time-of-flight (s). Hood angles and flywheel speeds are exposed as LoggedTunableNumbers so + // they can be edited live in tuning mode without redeploying. + private static final double[] mapDistances = {1.969, 2.185, 2.452, 3.701, 4.06, 4.68}; + private static final double[] defaultMapHoodAngles = { + Hood.minAngleDeg, Hood.minAngleDeg, Hood.minAngleDeg, 5.0, 8.0, 10.0 + }; + private static final double[] defaultMapFlywheelSpeeds = {31.0, 32.0, 33.0, 36.0, 37.75, 39.0}; + private static final double[] mapTimeOfFlights = {1.2, 1.2, 1.0, 1.61, 1.4, 1.1}; + private static final LoggedTunableNumber[] mapHoodAnglesDeg; + private static final LoggedTunableNumber[] mapFlywheelSpeedsRPS; + private static final int MAP_CONSUMER_ID = ShotCalculator.class.hashCode(); + static { minDistance = 1.34; maxDistance = 5.60; @@ -140,26 +153,20 @@ public static record LaunchPreset( passingMinDistance = 0.0; passingMaxDistance = 100000; - hoodAngleMap.put(1.969, Rotation2d.fromDegrees(Hood.minAngleDeg)); - hoodAngleMap.put(2.185, Rotation2d.fromDegrees(Hood.minAngleDeg)); - hoodAngleMap.put(2.452, Rotation2d.fromDegrees(Hood.minAngleDeg)); - hoodAngleMap.put(3.701, Rotation2d.fromDegrees(5.0)); - hoodAngleMap.put(4.06, Rotation2d.fromDegrees(8.0)); - hoodAngleMap.put(4.68, Rotation2d.fromDegrees(10.0)); - - flywheelSpeedMap.put(1.969, 31.0); - flywheelSpeedMap.put(2.185, 32.0); - flywheelSpeedMap.put(2.452, 33.0); - flywheelSpeedMap.put(3.701, 36.0); - flywheelSpeedMap.put(4.06, 37.75); - flywheelSpeedMap.put(4.68, 39.0); - - timeOfFlightMap.put(1.969, 1.2); - timeOfFlightMap.put(2.185, 1.2); - timeOfFlightMap.put(2.452, 1.0); - timeOfFlightMap.put(3.701, 1.61); - timeOfFlightMap.put(4.06, 1.4); - timeOfFlightMap.put(4.68, 1.1); + mapHoodAnglesDeg = new LoggedTunableNumber[mapDistances.length]; + mapFlywheelSpeedsRPS = new LoggedTunableNumber[mapDistances.length]; + for (int i = 0; i < mapDistances.length; i++) { + mapHoodAnglesDeg[i] = + new LoggedTunableNumber( + String.format("ShotCalculator/Map/%.3fm/HoodAngleDeg", mapDistances[i]), + defaultMapHoodAngles[i]); + mapFlywheelSpeedsRPS[i] = + new LoggedTunableNumber( + String.format("ShotCalculator/Map/%.3fm/FlywheelSpeedRPS", mapDistances[i]), + defaultMapFlywheelSpeeds[i]); + timeOfFlightMap.put(mapDistances[i], mapTimeOfFlights[i]); + } + rebuildShootingMaps(); // TODO: tune passing maps passingHoodAngleMap.put(passingMinDistance, Rotation2d.fromDegrees(0.0)); @@ -207,6 +214,30 @@ public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } + /** Rebuilds the hood-angle and flywheel-speed interpolation maps from the current tunable values. */ + private static void rebuildShootingMaps() { + hoodAngleMap.clear(); + flywheelSpeedMap.clear(); + for (int i = 0; i < mapDistances.length; i++) { + hoodAngleMap.put(mapDistances[i], Rotation2d.fromDegrees(mapHoodAnglesDeg[i].get())); + flywheelSpeedMap.put(mapDistances[i], mapFlywheelSpeedsRPS[i].get()); + } + } + + /** + * Returns a Pose2d positioned {@code distance} metres in front of the alliance hub, aimed + * directly at it. Useful for driving to a known shot distance during tuning. + * + * @param distance shooter-to-hub distance in metres + * @return aimed Pose2d at the requested distance + */ + public static Pose2d getAimedPoseAtDistance(double distance) { + Translation2d hubCenterBlue = FieldConstants.Hub.topCenterPoint.toTranslation2d(); + Translation2d robotBlue = + new Translation2d(hubCenterBlue.getX() - distance, hubCenterBlue.getY()); + return getStationaryAimedPose(AllianceFlipUtil.apply(robotBlue)); + } + /** Toggles hub-preset override on/off. Returns the new state. */ public boolean toggleHubPresetOverride() { hubPresetOverride = !hubPresetOverride; @@ -248,6 +279,12 @@ public ShootingParameters getParameters() { return latestParameters; } + // Rebuild interpolation maps if any tunable map values have changed. + boolean mapChanged = false; + for (LoggedTunableNumber n : mapHoodAnglesDeg) mapChanged |= n.hasChanged(MAP_CONSUMER_ID); + for (LoggedTunableNumber n : mapFlywheelSpeedsRPS) mapChanged |= n.hasChanged(MAP_CONSUMER_ID); + if (mapChanged) rebuildShootingMaps(); + // Hub-preset override: ignore vision/pose, use preset values at hubPresetDistance and keep // the robot's current heading so there is no auto-rotation. if (hubPresetOverride) {