From 1f18489b3ce7e79cb9849d0f02a5dea8706d4b29 Mon Sep 17 00:00:00 2001 From: Trent Fox Date: Mon, 30 Mar 2026 00:00:50 -0500 Subject: [PATCH] Add auto-aim hood with interpolation table, tuning helpers, and 24:1 gear ratio Replace polynomial hood angle calculation with NT-tunable interpolation tables for both hood angle and shooter RPS based on distance to target. Add save-shot button, lock-distance toggle, tunable nudge step, iterative shoot-on-the-move compensation, and global hood angle offset for competition-day quick adjustments. Fix hood gear ratio (16T:24T -> 10T:160T = 24:1) and soft limits to use Constants NT values. Fix pre-existing SHOOT_MANUAL compile error. Co-Authored-By: Claude Opus 4.6 --- CLAUDE.md | 56 +++ docs/HOOD_TUNING_GUIDE.md | 144 ++++++++ .../robot/commands/AutonomousCommands.java | 2 +- .../swrobotics/robot/config/Constants.java | 7 +- .../com/swrobotics/robot/control/AimCalc.java | 328 ++++++++++-------- .../swrobotics/robot/logging/Telemetry.java | 8 + .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../shooter/hood/HoodSubsystem.java | 46 ++- 8 files changed, 437 insertions(+), 156 deletions(-) create mode 100644 CLAUDE.md create mode 100644 docs/HOOD_TUNING_GUIDE.md diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..43cf43e --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,56 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project Overview + +FRC Team 2129 (Southwest Robotics) 2026 competition robot code. Java 17, WPILib 2026.2.1, command-based framework. + +## Build Commands + +```bash +./gradlew build # Compile and package +./gradlew deploy # Deploy to RoboRIO +./gradlew simulateJava # Run robot simulation (use simulationDebug for debug mode) +./gradlew test # Run unit tests +./gradlew clean # Clean build artifacts +``` + +## Architecture + +### Package Structure + +- `com.swrobotics.robot` — Robot code (entry point: `Main.java` → `Robot.java` → `RobotContainer.java`) +- `com.swrobotics.robot.subsystems` — Subsystems: swerve drive, shooter, hood, intake, indexer, expansion, vision +- `com.swrobotics.robot.commands` — Autonomous commands, drive commands, characterization +- `com.swrobotics.robot.config` — `Constants.java` (tunable values), `IOAllocation.java` (CAN IDs), `FieldPositions.java` +- `com.swrobotics.robot.control` — Controller mappings (`ControlBoard.java`), aim calculations, drive filtering +- `com.swrobotics.robot.logging` — Telemetry, field visualization, robot state visualization +- `com.swrobotics.lib` — Reusable utility library (NetworkTable wrappers, CTRE config helpers, field geometry, math) + +### Key Patterns + +- **Subsystem state machines**: Each subsystem uses an enum `State` with a `periodic()` method that switches on current state. State is set via public methods called by commands. +- **Tunable constants**: `NTEntry` (NTDouble, NTBoolean, etc.) wraps NetworkTables for runtime-adjustable parameters. Defined in `Constants.java`. +- **CTRE config helpers**: `TalonFXConfigHelper` / `TalonFXSConfigHelper` in `com.swrobotics.lib.ctre` wrap Phoenix 6 motor configuration with retry logic and apply patterns. +- **IOAllocation.CanId**: Stores device IDs with bus name; has factory methods like `.talonFX()`, `.talonFXS()`, `.cancoder()`, `.canrange()` to create devices directly. +- **Vision fusion**: `VisionSubsystem` manages 3 Limelight cameras feeding MegaTag2 pose estimates into swerve drive odometry, gated by robot speed. + +### Hardware + +- **Drive**: CTRE swerve (4x Kraken X60 FOC modules + Pigeon2 IMU) on "Gerald" CANivore bus +- **Shooter**: Dual Kraken motors (leader/follower), hood with position control +- **Intake/Indexer**: 5 motors total, CANrange ball sensor +- **Vision**: 3x Limelight cameras (front, right, back) + +### Autonomous + +PathPlanner-based. Auto routines in `src/main/deploy/pathplanner/autos/`, paths in `paths/`. Named commands registered in `AutonomousCommands.java`. Auto mode selected via SmartDashboard chooser. + +### Dependencies + +- WPILib 2026.2.1 (command-based framework) +- CTRE Phoenix 6 (motor control, swerve) +- PathPlanner (autonomous trajectories) +- Limelight (vision/AprilTag localization) +- JAMA 1.0.3 (matrix math) diff --git a/docs/HOOD_TUNING_GUIDE.md b/docs/HOOD_TUNING_GUIDE.md new file mode 100644 index 0000000..f0d8e63 --- /dev/null +++ b/docs/HOOD_TUNING_GUIDE.md @@ -0,0 +1,144 @@ +# Hood Auto-Aim Tuning Guide + +This guide walks through how to tune the adjustable hood so it automatically sets the correct angle based on distance to the target. The system uses an interpolation table — you teach it the correct hood angle at several known distances, and it smoothly blends between them. + +## What You Need + +- Robot deployed with latest code +- Shuffleboard or SmartDashboard connected +- Game pieces to shoot +- Tape measure (or use known field markings) +- Clear line of sight to AprilTags (for vision-based distance) + +## Shuffleboard Layout Setup + +Before tuning, add these key entries to your Shuffleboard layout: + +**Must have (drag these from NetworkTables):** +- `Shooter/DistanceToHub` — live distance to target in meters +- `Shooter/Hood Actual (deg)` — current hood angle from encoder +- `Shooter/Hood Mode` — current mode (MANUAL, AUTO_TRACK, etc.) +- `ShooterMech2d` — visual widget showing hood angle + distance bar + +**Tuning controls:** +- `Shooter/Aim/Tuning/Lock Distance` — checkbox, freezes distance reading +- `Shooter/Aim/Tuning/Save Shot` — button, saves current shot data +- `Shooter/Aim/Tuning/Next Save Slot` — shows which slot will be written next (0-5) +- `Shooter/Hood/Nudge Step (deg)` — how many degrees each nudge adjusts (default 0.5) + +**Interpolation table (auto-populated, you'll see values update as you save):** +- `Shooter/Aim/Hood Point 0-5 Dist (m)` +- `Shooter/Aim/Hood Point 0-5 Angle (deg)` +- `Shooter/Aim/RPS Point 0-5 Dist (m)` +- `Shooter/Aim/RPS Point 0-5 RPS` + +**Fine tuning:** +- `Shooter/Aim/Hood Angle Offset (deg)` — shifts ALL angles up/down by a fixed amount +- `Shooter/Aim/Base Flight Time (s)` — for shoot-on-the-move compensation +- `Shooter/Aim/Flight Time Per Meter (s)` — for shoot-on-the-move compensation + +## Step-by-Step Tuning Procedure + +### Phase 1: Collect Data Points (The Main Tuning Loop) + +You have 6 slots (Point 0 through Point 5). Start close to the target and work outward. + +**For each distance:** + +1. **Position the robot** at a known distance from the target. Use a tape measure or known field markings. Start at your closest expected shooting distance (~1m) and work out to your farthest (~3.5m). + +2. **Verify the distance reading.** Check `Shooter/DistanceToHub` in Shuffleboard. Make sure AprilTags are visible and the number looks reasonable. + +3. **Lock the distance.** Toggle `Shooter/Aim/Tuning/Lock Distance` to ON. This freezes the distance value so vision jitter doesn't shift it while you tune. You'll see `Shooter/DistanceToHub` stay fixed. + +4. **Switch hood to MANUAL mode.** Use the controller or set `Shooter/Hood Mode` so you have direct control of the hood angle. + +5. **Adjust the hood angle.** Use the nudge up/down buttons on the controller to move the hood in small increments. Watch `Shooter/Hood Actual (deg)` in Shuffleboard. The step size is controlled by `Shooter/Hood/Nudge Step (deg)`: + - Start at **2.0 deg** to get in the ballpark quickly + - Switch to **0.5 deg** (default) for fine-tuning once you're close + +6. **Shoot and observe.** Fire test shots. Adjust the hood angle until shots consistently hit the target. Take your time here — this is the most important step. + +7. **Save the shot.** Once shots are landing consistently, toggle `Shooter/Aim/Tuning/Save Shot` to ON. This automatically records: + - The current locked distance into the hood and RPS distance fields + - The actual hood encoder angle into the hood angle field + - The current shooter RPS into the RPS field + + The `Next Save Slot` counter advances automatically. + +8. **Unlock the distance.** Toggle `Shooter/Aim/Tuning/Lock Distance` back to OFF. + +9. **Move to the next distance** and repeat from step 1. + +### Suggested Distances + +For 6 data points, a good spread is: + +| Slot | Distance | Notes | +|------|----------|-------| +| 0 | ~1.0 m | Closest shot (near fender) | +| 1 | ~1.5 m | Close-mid range | +| 2 | ~2.0 m | Mid range | +| 3 | ~2.5 m | Mid-far range | +| 4 | ~3.0 m | Far range | +| 5 | ~3.5 m | Maximum shooting distance | + +You don't need to be at these exact distances. The system interpolates between whatever distances you record. + +### Phase 2: Test Auto-Tracking + +1. **Switch hood to AUTO_TRACK mode.** The hood will now automatically adjust based on distance. + +2. **Drive around at various distances** and shoot. The interpolation table fills in angles between your recorded points. + +3. **Check the edges.** Pay special attention to distances at or beyond your closest and farthest recorded points. The system clamps to the nearest endpoint outside the table range. The `Shooter/Aim/In Range` indicator tells you whether you're within the table bounds. + +### Phase 3: Competition-Day Quick Adjust + +If shots are consistently off at competition (different game pieces, field conditions, etc.), use the **Hood Angle Offset** instead of re-tuning every point: + +- `Shooter/Aim/Hood Angle Offset (deg)` shifts every calculated angle by a fixed amount +- If shots are going high, decrease the offset (negative) +- If shots are going low, increase the offset (positive) +- This is one number that fixes everything — much faster than re-tuning individual points + +### Phase 4: Shooter RPS Tuning (Optional) + +The RPS (flywheel speed) table works the same way as the hood angle table. The Save Shot button records both simultaneously. If you want to tune RPS independently: + +1. Edit `Shooter/Aim/RPS Point X Dist (m)` and `Shooter/Aim/RPS Point X RPS` directly in Shuffleboard +2. Higher distances may need higher RPS to maintain shot accuracy +3. All RPS values are in rotations per second of the flywheel + +## Troubleshooting + +| Problem | Check | +|---------|-------| +| Distance reading is 0 or jumping wildly | No AprilTags visible to Limelights. Reposition robot so cameras can see tags. | +| Hood doesn't move in MANUAL | Is it still in HOMING mode? Wait for homing to complete (current spike detection). | +| Save Shot doesn't seem to work | Check `Next Save Slot` — it may have wrapped to 0 and overwritten your first point. | +| AUTO_TRACK angle seems wrong | Check `Shooter/Aim/In Range`. If false, you're outside your recorded distances. | +| Shots were good in practice but off at comp | Use `Hood Angle Offset (deg)` — one knob to shift everything. | +| Hood overshoots or oscillates | Tune PID at `Shooter/Hood/PID` — try reducing kP or increasing kD. | +| Need more than 6 data points | Change `NUM_POINTS` in `AimCalc.java` and redeploy. | + +## All Tunable Parameters Reference + +| NetworkTables Path | Default | Purpose | +|---|---|---| +| `Shooter/Hood/Nudge Step (deg)` | 0.5 | Degrees per nudge button press | +| `Shooter/Hood/Max Angle (Deg)` | 40.0 | Soft upper limit for hood angle | +| `Shooter/Hood/Min Angle (Deg)` | 22.7 | Soft lower limit for hood angle | +| `Shooter/Hood/PID` | kP=6.0, kI=0.8 | Hood position PID gains | +| `Shooter/Aim/Hood Angle Offset (deg)` | 0.0 | Global angle shift (competition quick-fix) | +| `Shooter/Aim/Base Flight Time (s)` | 0.4 | Shoot-on-the-move: base projectile travel time | +| `Shooter/Aim/Flight Time Per Meter (s)` | 0.10 | Shoot-on-the-move: additional time per meter | +| `Shooter/Aim/Hood Point X Dist (m)` | varies | Interpolation table distance entries | +| `Shooter/Aim/Hood Point X Angle (deg)` | varies | Interpolation table angle entries | +| `Shooter/Aim/RPS Point X Dist (m)` | varies | Interpolation table RPS distance entries | +| `Shooter/Aim/RPS Point X RPS` | varies | Interpolation table RPS entries | +| `Shooter/Aim/Tuning/Lock Distance` | false | Freeze distance for stable tuning | +| `Shooter/Aim/Tuning/Save Shot` | false | One-click save current shot data | +| `Shooter/Aim/Tuning/Next Save Slot` | 0 | Which slot Save Shot writes to next | + +All values marked `.setPersistent()` survive robot reboots — you only need to tune once. diff --git a/src/main/java/com/swrobotics/robot/commands/AutonomousCommands.java b/src/main/java/com/swrobotics/robot/commands/AutonomousCommands.java index d1e9daf..06d8e63 100644 --- a/src/main/java/com/swrobotics/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/swrobotics/robot/commands/AutonomousCommands.java @@ -12,7 +12,7 @@ public class AutonomousCommands { public static Command getShootCommand(RobotContainer robot) { - return robot.shooter.commandSetState(ShooterSubsystem.State.SHOOT_MANUAL) + return robot.shooter.commandSetState(ShooterSubsystem.State.SHOOT) .withTimeout(.75) .andThen(robot.indexer.commandSetState(IndexerSubsystem.State.FEED)).withTimeout(5); } diff --git a/src/main/java/com/swrobotics/robot/config/Constants.java b/src/main/java/com/swrobotics/robot/config/Constants.java index 839ef7f..8dc4a16 100644 --- a/src/main/java/com/swrobotics/robot/config/Constants.java +++ b/src/main/java/com/swrobotics/robot/config/Constants.java @@ -206,7 +206,12 @@ public final class Constants { public static final NTEntry kHoodCruiseVelocity = new NTDouble("Shooter/Hood/Cruise Velocity", 60.0).setPersistent(); public static final NTEntry kHoodAcceleration = new NTDouble("Shooter/Hood/Acceleration", 120.0).setPersistent(); public static final NTSlot0Configs kHoodPID = new NTSlot0Configs("Shooter/Hood/PID", 6.0, 0.8, 0.0, 0.0, 0.1, 0.0); - + + /* --- Aim Calc --- */ + public static final NTEntry kBaseFlightTime = new NTDouble("Shooter/Aim/Base Flight Time (s)", 0.4).setPersistent(); + public static final NTEntry kFlightTimePerMeter = new NTDouble("Shooter/Aim/Flight Time Per Meter (s)", 0.10).setPersistent(); + public static final NTEntry kHoodAngleOffset = new NTDouble("Shooter/Aim/Hood Angle Offset (deg)", 0.0).setPersistent(); + /* --- Indexer --- */ public static final NTEntry kIndexerIdleVoltage = new NTDouble("Intake/Indexer/Idle Voltage", 0.0).setPersistent(); diff --git a/src/main/java/com/swrobotics/robot/control/AimCalc.java b/src/main/java/com/swrobotics/robot/control/AimCalc.java index b42eaee..cf18eac 100644 --- a/src/main/java/com/swrobotics/robot/control/AimCalc.java +++ b/src/main/java/com/swrobotics/robot/control/AimCalc.java @@ -1,134 +1,165 @@ -// package com.swrobotics.robot.control; - -// import java.util.TreeMap; - -// import com.swrobotics.robot.config.Constants; -// import com.swrobotics.robot.config.FieldPositions; - -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.wpilibj.DriverStation; - -// public class AimCalc { -// private static final AimCalc instance = new AimCalc(); -// public static AimCalc getInstance() { return instance; } - - -// public static double kConstantShooterRPS = 20.0; - -// public static double kBaseFlightTime = 0.4; -// public static double kFlightTimePerMeter = 0.10; - -// private final TreeMap hoodMap = new TreeMap<>(); - -// private Rotation2d drivebaseAimAngle = new Rotation2d(); -// private Rotation2d hoodAngle = new Rotation2d(); -// private double shooterRPS = kConstantShooterRPS; -// private double lastDistanceToHub = 0.0; - -// private AimCalc() { - -// hoodMap.put(1.0, 30.0); -// hoodMap.put(1.25, 35.0); -// hoodMap.put(1.5, 40.0); -// hoodMap.put(1.75, 45.0); -// } - -// public void update(Pose2d robotPose, double fieldVx, double fieldVy) { -// Translation2d hubTarget = FieldPositions.getAllianceHubPose( -// DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) -// ).getTranslation(); - -// Translation2d shooterOffset = new Translation2d( -// Constants.kShooterOffsetX, -// Constants.kShooterOffsetY -// ).rotateBy(robotPose.getRotation()); - -// Translation2d shooterFieldPos = robotPose.getTranslation().plus(shooterOffset); - -// double actualDist = shooterFieldPos.getDistance(hubTarget); -// lastDistanceToHub = actualDist; - -// double shotTime = estimateFlightTime(actualDist); - -// Translation2d robotMotionDuringShot = new Translation2d( -// fieldVx * shotTime, -// fieldVy * shotTime -// ); -// Translation2d virtualTarget = hubTarget.minus(robotMotionDuringShot); - -// Translation2d delta = virtualTarget.minus(shooterFieldPos); - -// drivebaseAimAngle = delta.getAngle(); -// double vDist = delta.getNorm(); - -// shooterRPS = kConstantShooterRPS; -// hoodAngle = Rotation2d.fromDegrees(getInterpolatedHoodAngle(vDist)); -// } - -// private double estimateFlightTime(double distanceMeters) { -// return kBaseFlightTime + kFlightTimePerMeter * distanceMeters; -// } - -// private double getInterpolatedHoodAngle(double distance) { -// double minKey = hoodMap.firstKey(); -// double maxKey = hoodMap.lastKey(); -// double clamped = Math.max(minKey, Math.min(maxKey, distance)); - -// Double lowKey = hoodMap.floorKey(clamped); -// Double highKey = hoodMap.ceilingKey(clamped); - -// if (lowKey == null && highKey == null) { -// return 0.0; -// } -// if (lowKey == null) return hoodMap.get(highKey); -// if (highKey == null) return hoodMap.get(lowKey); -// if (lowKey.equals(highKey)) return hoodMap.get(lowKey); - -// double lowVal = hoodMap.get(lowKey); -// double highVal = hoodMap.get(highKey); -// double t = (clamped - lowKey) / (highKey - lowKey); - -// return lowVal + t * (highVal - lowVal); -// } - -// public Rotation2d getDrivebaseAimAngle() { return drivebaseAimAngle; } -// public Rotation2d getHoodAngle() { return hoodAngle; } -// public double getShooterRPS() { return shooterRPS; } -// public double getLastDistanceToHub() { return lastDistanceToHub; } -// } package com.swrobotics.robot.control; +import com.swrobotics.lib.net.NTBoolean; +import com.swrobotics.lib.net.NTDouble; +import com.swrobotics.lib.net.NTEntry; +import com.swrobotics.lib.net.NTInteger; import com.swrobotics.robot.config.Constants; import com.swrobotics.robot.config.FieldPositions; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.DriverStation; public class AimCalc { private static final AimCalc instance = new AimCalc(); public static AimCalc getInstance() { return instance; } - public static double kConstantShooterRPS = 20.0; + // Number of interpolation data points (tune count here if needed) + private static final int NUM_POINTS = 6; + + // Default (distance_m, hood_angle_deg) pairs — starting guesses, tune on field + private static final double[][] DEFAULT_HOOD_POINTS = { + {1.0, 25.0}, + {1.5, 28.0}, + {2.0, 31.0}, + {2.5, 34.0}, + {3.0, 37.0}, + {3.5, 40.0}, + }; + + // Default (distance_m, shooter_rps) pairs — starting guesses, tune on field + private static final double[][] DEFAULT_RPS_POINTS = { + {1.0, 18.0}, + {1.5, 19.0}, + {2.0, 20.0}, + {2.5, 21.0}, + {3.0, 22.0}, + {3.5, 24.0}, + }; + + // NT-tunable data points for hood angle interpolation + private final NTEntry[] hoodDists = new NTEntry[NUM_POINTS]; + private final NTEntry[] hoodAngles = new NTEntry[NUM_POINTS]; + + // NT-tunable data points for shooter RPS interpolation + private final NTEntry[] rpsDists = new NTEntry[NUM_POINTS]; + private final NTEntry[] rpsValues = new NTEntry[NUM_POINTS]; + + // Interpolation maps rebuilt from NT values + private InterpolatingDoubleTreeMap hoodMap = new InterpolatingDoubleTreeMap(); + private InterpolatingDoubleTreeMap rpsMap = new InterpolatingDoubleTreeMap(); + + // Output values + private Rotation2d drivebaseAimAngle = new Rotation2d(); + private Rotation2d hoodAngle = new Rotation2d(); + private double shooterRPS = 20.0; + private double lastDistanceToHub = 0.0; + private double lastVirtualDistance = 0.0; + private boolean inRange = false; + + // Track min/max keys for range checking + private double hoodMapMinDist = 0.0; + private double hoodMapMaxDist = 0.0; + + // --- Tuning helpers --- + // "Save Shot" button: records current (distance, hood angle, RPS) into the next point slot + private final NTEntry saveShotTrigger = new NTBoolean("Shooter/Aim/Tuning/Save Shot", false); + private final NTEntry nextSaveSlot = new NTDouble("Shooter/Aim/Tuning/Next Save Slot", 0); + // "Lock Distance" toggle: freezes distance reading so it doesn't jitter while tuning + private final NTEntry lockDistance = new NTBoolean("Shooter/Aim/Tuning/Lock Distance", false); + private double lockedDistance = 0.0; + + private AimCalc() { + // Create NT entries for hood angle data points + for (int i = 0; i < NUM_POINTS; i++) { + hoodDists[i] = new NTDouble("Shooter/Aim/Hood Point " + i + " Dist (m)", DEFAULT_HOOD_POINTS[i][0]).setPersistent(); + hoodAngles[i] = new NTDouble("Shooter/Aim/Hood Point " + i + " Angle (deg)", DEFAULT_HOOD_POINTS[i][1]).setPersistent(); + + rpsDists[i] = new NTDouble("Shooter/Aim/RPS Point " + i + " Dist (m)", DEFAULT_RPS_POINTS[i][0]).setPersistent(); + rpsValues[i] = new NTDouble("Shooter/Aim/RPS Point " + i + " RPS", DEFAULT_RPS_POINTS[i][1]).setPersistent(); + + // Rebuild maps when any value changes + hoodDists[i].onChange(this::rebuildMaps); + hoodAngles[i].onChange(this::rebuildMaps); + rpsDists[i].onChange(this::rebuildMaps); + rpsValues[i].onChange(this::rebuildMaps); + } + + // Also rebuild when offset changes + Constants.kHoodAngleOffset.onChange(this::rebuildMaps); + + // Save shot: when toggled true, record current state into next slot + saveShotTrigger.onChange(() -> { + if (saveShotTrigger.get()) { + saveCurrentShot(); + saveShotTrigger.set(false); + } + }); + + rebuildMaps(); + } + /** + * Records the current distance, hood angle, and shooter RPS into the next + * available interpolation point slot. Called from the "Save Shot" button. + */ + public void saveCurrentShot(double distanceM, double angleDeg, double rps) { + int slot = (int) Math.round(nextSaveSlot.get()); + if (slot < 0 || slot >= NUM_POINTS) { + slot = 0; // Wrap around + } + + hoodDists[slot].set(distanceM); + hoodAngles[slot].set(angleDeg); + rpsDists[slot].set(distanceM); + rpsValues[slot].set(rps); + + // Advance to next slot + nextSaveSlot.set((double) ((slot + 1) % NUM_POINTS)); + rebuildMaps(); + } - // Base flight time offset (seconds) - public static double kBaseFlightTime = 0.4; - public static double kFlightTimePerMeter = 0.10; + /** Save using current live values (called from NT button) */ + private void saveCurrentShot() { + saveCurrentShot(lastDistanceToHub, currentHoodAngleDeg, shooterRPS); + } + /** Set by HoodSubsystem so save-shot can capture the actual hood angle */ + private double currentHoodAngleDeg = 0.0; + public void setCurrentHoodAngleDeg(double deg) { + currentHoodAngleDeg = deg; + } - public static double kHoodCurveA = 0.0; // (Curve steepness) - public static double kHoodCurveB = 5.0; //(Degrees added per meter) - public static double kHoodCurveC = 22.7; //(Base angle at 0 meters) + private void rebuildMaps() { + InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap(); + InterpolatingDoubleTreeMap newRpsMap = new InterpolatingDoubleTreeMap(); - private Rotation2d drivebaseAimAngle = new Rotation2d(); - private Rotation2d hoodAngle = new Rotation2d(); - private double shooterRPS = kConstantShooterRPS; - private double lastDistanceToHub = 0.0; + double minDist = Double.MAX_VALUE; + double maxDist = -Double.MAX_VALUE; + + for (int i = 0; i < NUM_POINTS; i++) { + double dist = hoodDists[i].get(); + double angle = hoodAngles[i].get(); + newHoodMap.put(dist, angle); + + minDist = Math.min(minDist, dist); + maxDist = Math.max(maxDist, dist); + } + for (int i = 0; i < NUM_POINTS; i++) { + double dist = rpsDists[i].get(); + double rps = rpsValues[i].get(); + newRpsMap.put(dist, rps); + } + + hoodMap = newHoodMap; + rpsMap = newRpsMap; + hoodMapMinDist = minDist; + hoodMapMaxDist = maxDist; + } public void update(Pose2d robotPose, double fieldVx, double fieldVy) { // Alliance-relative hub position @@ -136,56 +167,67 @@ public void update(Pose2d robotPose, double fieldVx, double fieldVy) { DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) ).getTranslation(); - // Shooter offset from robot center + // Shooter offset from robot center (rotated to field frame) Translation2d shooterOffset = new Translation2d( Constants.kShooterOffsetX, Constants.kShooterOffsetY ).rotateBy(robotPose.getRotation()); Translation2d shooterFieldPos = robotPose.getTranslation().plus(shooterOffset); - // Distance from shooter to hub (static / instantaneous) - double staticDist = shooterFieldPos.getDistance(hubTarget); + + // Static distance from shooter to hub + double rawDist = shooterFieldPos.getDistance(hubTarget); + + // Lock distance: freeze the reading for stable tuning + if (lockDistance.get()) { + if (lockedDistance == 0.0) { + lockedDistance = rawDist; // Capture on first lock + } + } else { + lockedDistance = rawDist; + } + double staticDist = lockedDistance; lastDistanceToHub = staticDist; - // Estimate flight time based on distance - double shotTime = estimateFlightTime(staticDist); + // Iterative shoot-on-the-move compensation (converges in ~5 iterations) + double baseFlightTime = Constants.kBaseFlightTime.get(); + double flightTimePerMeter = Constants.kFlightTimePerMeter.get(); - // Compensate for robot motion: virtual target - Translation2d robotMotionDuringShot = new Translation2d( - fieldVx * shotTime, - fieldVy * shotTime - ); - Translation2d virtualTarget = hubTarget.minus(robotMotionDuringShot); + Translation2d virtualTarget = hubTarget; + double vDist = staticDist; - Translation2d delta = virtualTarget.minus(shooterFieldPos); + for (int i = 0; i < 5; i++) { + double shotTime = baseFlightTime + flightTimePerMeter * vDist; + Translation2d robotMotionDuringShot = new Translation2d( + fieldVx * shotTime, + fieldVy * shotTime + ); + virtualTarget = hubTarget.minus(robotMotionDuringShot); + Translation2d delta = virtualTarget.minus(shooterFieldPos); + vDist = delta.getNorm(); + } + + lastVirtualDistance = vDist; + // Drivebase aim angle (direction robot should face) + Translation2d delta = virtualTarget.minus(shooterFieldPos); drivebaseAimAngle = delta.getAngle(); - double vDist = delta.getNorm(); // Virtual distance for shoot-on-the-move - // 1. Lock the shooter to the constant RPS - shooterRPS = kConstantShooterRPS; - - // 2. Calculate the hood angle using the polynomial curve - hoodAngle = Rotation2d.fromDegrees(calculateHoodAngle(vDist)); - } + // Check if distance is within the interpolation table range + inRange = vDist >= hoodMapMinDist && vDist <= hoodMapMaxDist; - private double estimateFlightTime(double distanceMeters) { - // TUNING: adjust kBaseFlightTime and kFlightTimePerMeter - return kBaseFlightTime + kFlightTimePerMeter * distanceMeters; - } + // Look up hood angle from interpolation table + apply global offset + double hoodDeg = hoodMap.get(vDist) + Constants.kHoodAngleOffset.get(); + hoodAngle = Rotation2d.fromDegrees(hoodDeg); - private double calculateHoodAngle(double distance) { - // Quadratic equation to calculate the exact angle needed - // y = ax^2 + bx + c - double targetDegrees = (kHoodCurveA * (distance * distance)) - + (kHoodCurveB * distance) - + kHoodCurveC; - return targetDegrees; + // Look up shooter RPS from interpolation table + shooterRPS = rpsMap.get(vDist); } - public Rotation2d getDrivebaseAimAngle() { return drivebaseAimAngle; } public Rotation2d getHoodAngle() { return hoodAngle; } public double getShooterRPS() { return shooterRPS; } public double getLastDistanceToHub() { return lastDistanceToHub; } -} \ No newline at end of file + public double getLastVirtualDistance() { return lastVirtualDistance; } + public boolean isInRange() { return inRange; } +} diff --git a/src/main/java/com/swrobotics/robot/logging/Telemetry.java b/src/main/java/com/swrobotics/robot/logging/Telemetry.java index dabefe9..a295c12 100644 --- a/src/main/java/com/swrobotics/robot/logging/Telemetry.java +++ b/src/main/java/com/swrobotics/robot/logging/Telemetry.java @@ -12,6 +12,10 @@ public final class Telemetry { private static final MechanismRoot2d root = mech.getRoot("shooter", 1.5, 0.2); private static final MechanismLigament2d hoodLig = root.append(new MechanismLigament2d("hood", 1.0, 0.0)); // length 1, angle 0 + // Distance label ligament — length represents distance visually + private static final MechanismRoot2d distRoot = mech.getRoot("distLabel", 0.2, 2.5); + private static final MechanismLigament2d distLig = + distRoot.append(new MechanismLigament2d("distance", 0.0, 0.0)); private Telemetry() {} @@ -28,10 +32,14 @@ public static void periodic() { // Update mech graphic hoodLig.setAngle(hoodDeg); + // Scale distance into the mech widget (0-5m maps to 0-2.5 length) + distLig.setLength(Math.min(distance / 2.0, 2.5)); // Scalars for logging/analysis SmartDashboard.putNumber("Shooter/DistanceToHub", distance); SmartDashboard.putNumber("Shooter/HoodAngleDeg", hoodDeg); SmartDashboard.putNumber("Shooter/TargetRPS", targetRps); + SmartDashboard.putNumber("Shooter/Aim/Virtual Distance", aim.getLastVirtualDistance()); + SmartDashboard.putBoolean("Shooter/Aim/In Range", aim.isInRange()); } } \ No newline at end of file diff --git a/src/main/java/com/swrobotics/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/com/swrobotics/robot/subsystems/shooter/ShooterSubsystem.java index 442cc68..4f06490 100644 --- a/src/main/java/com/swrobotics/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/com/swrobotics/robot/subsystems/shooter/ShooterSubsystem.java @@ -54,7 +54,7 @@ public void periodic() { break; case SHOOT: - currentTargetRPS = 20; + currentTargetRPS = AimCalc.getInstance().getShooterRPS(); break; case WARM: currentTargetRPS = 10.0; // TUNE diff --git a/src/main/java/com/swrobotics/robot/subsystems/shooter/hood/HoodSubsystem.java b/src/main/java/com/swrobotics/robot/subsystems/shooter/hood/HoodSubsystem.java index d3eeca4..2f781c1 100644 --- a/src/main/java/com/swrobotics/robot/subsystems/shooter/hood/HoodSubsystem.java +++ b/src/main/java/com/swrobotics/robot/subsystems/shooter/hood/HoodSubsystem.java @@ -1,13 +1,13 @@ package com.swrobotics.robot.subsystems.shooter.hood; -import java.lang.Thread.State; - import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.swrobotics.lib.ctre.TalonFXConfigHelper; +import com.swrobotics.lib.net.NTDouble; +import com.swrobotics.lib.net.NTEntry; import com.swrobotics.robot.config.Constants; import com.swrobotics.robot.config.IOAllocation; import com.swrobotics.robot.control.AimCalc; @@ -19,12 +19,16 @@ public class HoodSubsystem extends SubsystemBase { // --- HOOD CONFIGURATIONS --- - public static final double kSensorToMechRatio = 1.0; // TUNE: Enter your gear ratio here - public static final double kMaxAngleDeg = 45.0; // TUNE: Maximum safe hood angle - public static final double kMinAngleDeg = 0.0; // TUNE: Minimum safe hood angle + // Gear ratio: 16T->24T then 10T->160T = (24/16) * (160/10) = 24:1 + public static final double kSensorToMechRatio = 24.0; + public static final double kMaxAngleDeg = 90.0; // Physical hard limit (hood quarter turn) + public static final double kMinAngleDeg = 0.0; // Home position (hard stop) public static final double kHomingVoltage = -2.0; // TUNE: Voltage to drive hood down (- for down) public static final double kHomingCurrentLimit = 15.0; // TUNE: Stator current spike (Amps) to detect hard stop + // Tunable nudge step for manual adjustment (smaller = finer tuning) + private static final NTEntry kNudgeStepDeg = new NTDouble("Shooter/Hood/Nudge Step (deg)", 0.5).setPersistent(); + private final TalonFX motor; private final PositionVoltage positionControl = new PositionVoltage(0.0).withEnableFOC(true); private final VoltageOut voltageControl = new VoltageOut(0.0); @@ -69,18 +73,25 @@ public void periodic() { } // 2. Enforce Min/Max Limits constraints on ALL position modes - double maxRotations = kMaxAngleDeg / 360.0; - double minRotations = kMinAngleDeg / 360.0; + double maxRotations = Constants.kHoodMaxAngle.get() / 360.0; + double minRotations = Constants.kHoodMinAngle.get() / 360.0; targetRotations = Math.max(minRotations, Math.min(maxRotations, targetRotations)); // 3. Command the Motor motor.setControl(positionControl.withPosition(targetRotations)); - // 4. Telemetry + // 4. Feed actual angle back to AimCalc (for save-shot feature) + double actualRotations = motor.getPosition().getValueAsDouble(); + double actualDeg = actualRotations * 360.0; + AimCalc.getInstance().setCurrentHoodAngleDeg(actualDeg); + + // 5. Telemetry SmartDashboard.putString("Shooter/Hood Mode", mode.name()); SmartDashboard.putBoolean("Shooter/Hood At Target", isAtTarget()); + SmartDashboard.putNumber("Shooter/Hood Target (deg)", targetRotations * 360.0); + SmartDashboard.putNumber("Shooter/Hood Actual (deg)", actualDeg); SmartDashboard.putNumber("Shooter/Hood Target Rot", targetRotations); - SmartDashboard.putNumber("Shooter/Hood Actual Rot", motor.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Shooter/Hood Actual Rot", actualRotations); SmartDashboard.putNumber("Shooter/Hood Current", motor.getStatorCurrent().getValueAsDouble()); } @@ -106,7 +117,7 @@ public Command setMode(HoodMode newMode) { return Commands.runOnce(() -> mode = newMode, this); } - // ------------- MANUAL NUDGE (±2°) ------------- + // ------------- MANUAL NUDGE ------------- public void nudgeAngleDegrees(double deltaDeg) { double deltaRot = deltaDeg / 360.0; targetRotations += deltaRot; @@ -116,4 +127,19 @@ public void nudgeAngleDegrees(double deltaDeg) { public Command commandNudgeAngleDegrees(double deltaDeg) { return Commands.runOnce(() -> nudgeAngleDegrees(deltaDeg), this); } + + /** Nudge up by the tunable step size */ + public Command commandNudgeUp() { + return Commands.runOnce(() -> nudgeAngleDegrees(kNudgeStepDeg.get()), this); + } + + /** Nudge down by the tunable step size */ + public Command commandNudgeDown() { + return Commands.runOnce(() -> nudgeAngleDegrees(-kNudgeStepDeg.get()), this); + } + + /** Get the current hood angle in degrees (from motor encoder) */ + public double getActualAngleDegrees() { + return motor.getPosition().getValueAsDouble() * 360.0; + } } \ No newline at end of file