diff --git a/.gitignore b/.gitignore index 5a31672..6f0d4dc 100644 --- a/.gitignore +++ b/.gitignore @@ -188,3 +188,8 @@ compile_commands.json # Build-time generated constants src/main/java/frc/robot/BuildConstants.java +src/main/java/frc/robot/.BuildConstants.java.swp + +# Local planning and AI assistant files (not for repo) +docs/ +.claude/ diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..94e3108 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,55 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Build Commands + +```bash +./gradlew build # Compile + run tests +./gradlew compileJava # Compile only (fastest check) +./gradlew deploy # Deploy to RoboRIO +``` + +CI runs `./gradlew build` on push/PR to Main using `wpilib/roborio-cross-ubuntu:2025-22.04`. + +## Architecture + +FRC Team 7461 (Sushi Squad) command-based robot using **WPILib 2026**, **AdvantageKit**, and **Phoenix6** (CTRE TalonFX/Kraken motors). + +### IO Abstraction Pattern + +Every subsystem uses an IO interface layer for hardware abstraction: +- `ShooterIO` (interface) → `ShooterIOKraken` (real hardware) / `ShooterIOSim` (simulation) +- `IntakeIO` → `IntakeReal` / `IntakeSim` +- `HopperIO` → `HopperIOReal` / `HopperIOSim` +- `ModuleIO` → `ModuleIOTalonFX` / `ModuleIOSim` +- `GyroIO` → `GyroIOPigeon2` / `GyroIONavX` + +`RobotContainer` selects real vs sim implementations based on `Robot.isReal()`. + +### State Machine + +`StateMachine.java` maps robot-level states (`IDLE`, `SHOOT_ONLY`, `INTAKE_DOWN`, `INTAKE_DOWN_AND_SHOOT`) to per-subsystem states for shooter, hopper, and intake. + +### Subsystems + +- **Swerve** — SDS MK4i L3 with KrakenX60, Pigeon2 gyro, PathPlanner autos +- **Shooter** — Flywheel (3:4 gear ratio, flywheel faster than motor) + hooded mechanism with MotionMagic + feeder +- **Intake** — Deployed pivot + rollers, TalonFX with MotionMagic +- **Hopper** — Simple transport motor +- **Vision** — Dual Limelights ("limelight-left" primary, "limelight-right" secondary), ProjectileSimulator for ballistics, ShotCalculator with LUT + +### Key Constants Location + +`src/main/java/frc/robot/generated/Constants.java` — auto-generated by gversion plugin. Subsystem-specific constants live in their respective subsystem files (e.g., `ShooterSubsystem.java`, `Intake.java`). + +### Vendor Libraries + +Phoenix6 26.1.1, AdvantageKit 26.0.0, PathplannerLib 2026.1.2, PhotonLib v2026.2.2, REVLib 2026.0.2. Vendordep JSONs are in `/vendordeps/`. + +## Conventions + +- Gear ratios are expressed as motor rotations per mechanism rotation (e.g., `FLYWHEEL_GEAR_RATIO = 3.0/4.0` means flywheel spins faster than motor) +- Shooter RPM API takes flywheel RPM; the gear ratio conversion happens inside `ShooterIOKraken` +- AdvantageKit `@AutoLog` annotation processor generates logging boilerplate +- Autonomous paths are PathPlanner `.path` files registered as named commands in `AutoCommands` diff --git a/src/main/deploy/pathplanner/autos/B1_Hub_HP.auto b/src/main/deploy/pathplanner/autos/B1_Hub_HP.auto deleted file mode 100644 index 709aed8..0000000 --- a/src/main/deploy/pathplanner/autos/B1_Hub_HP.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B3_up_to_hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Hub_to_HP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B1_Hub_HP_Shoot.auto b/src/main/deploy/pathplanner/autos/B1_Hub_HP_Shoot.auto deleted file mode 100644 index f64b0c1..0000000 --- a/src/main/deploy/pathplanner/autos/B1_Hub_HP_Shoot.auto +++ /dev/null @@ -1,67 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B1_up_to_hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Hub_to_HP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - }, - { - "type": "path", - "data": { - "pathName": "HP_to_Hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B2_Hub_HP.auto b/src/main/deploy/pathplanner/autos/B2_Hub_HP.auto deleted file mode 100644 index 709aed8..0000000 --- a/src/main/deploy/pathplanner/autos/B2_Hub_HP.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B3_up_to_hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Hub_to_HP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B2_Hub_HP_Shoot.auto b/src/main/deploy/pathplanner/autos/B2_Hub_HP_Shoot.auto deleted file mode 100644 index e3379d3..0000000 --- a/src/main/deploy/pathplanner/autos/B2_Hub_HP_Shoot.auto +++ /dev/null @@ -1,67 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B2_up_to_hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Hub_to_HP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - }, - { - "type": "path", - "data": { - "pathName": "HP_to_Hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B3_Hub_HP.auto b/src/main/deploy/pathplanner/autos/B3_Hub_HP.auto deleted file mode 100644 index 709aed8..0000000 --- a/src/main/deploy/pathplanner/autos/B3_Hub_HP.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B3_up_to_hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Hub_to_HP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B3_Hub_HP_Shoot.auto b/src/main/deploy/pathplanner/autos/B3_Hub_HP_Shoot.auto deleted file mode 100644 index c005a0e..0000000 --- a/src/main/deploy/pathplanner/autos/B3_Hub_HP_Shoot.auto +++ /dev/null @@ -1,67 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B3_up_to_hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Hub_to_HP" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - }, - { - "type": "path", - "data": { - "pathName": "HP_to_Hub" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Ideal_N_Shoot.auto b/src/main/deploy/pathplanner/autos/Ideal_N_Shoot.auto new file mode 100644 index 0000000..51f620a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Ideal_N_Shoot.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Trench_Neutral" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NT_AutoAlign" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoAlign" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 20.0 + } + }, + { + "type": "named", + "data": { + "name": "Idle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/N_Shoot.auto b/src/main/deploy/pathplanner/autos/N_Shoot.auto new file mode 100644 index 0000000..33ef10b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/N_Shoot.auto @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Trench_Neutral" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NT_Turn_Back" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B1_Set_AutoAlign" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoAlign" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 20.0 + } + }, + { + "type": "named", + "data": { + "name": "Idle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test_Ideal_N_Shoot.auto b/src/main/deploy/pathplanner/autos/Test_Ideal_N_Shoot.auto new file mode 100644 index 0000000..c49508b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test_Ideal_N_Shoot.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Trench_Neutral" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NT_Turn_Back" + } + }, + { + "type": "named", + "data": { + "name": "Down" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Test_Shoot_on_the_Move" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 20.0 + } + }, + { + "type": "named", + "data": { + "name": "Idle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B1_Set_AutoAlign.path b/src/main/deploy/pathplanner/paths/B1_Set_AutoAlign.path new file mode 100644 index 0000000..1e9a960 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B1_Set_AutoAlign.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3295666666666666, + "y": 7.328622222222221 + }, + "prevControl": null, + "nextControl": { + "x": 2.3161444444444443, + "y": 7.474788888888888 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0853209700427953, + "y": 3.9767760342368046 + }, + "prevControl": { + "x": 1.645406562054208, + "y": 3.912082738944366 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.88863040496074 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NT_AutoAlign.path b/src/main/deploy/pathplanner/paths/NT_AutoAlign.path new file mode 100644 index 0000000..2a6b896 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NT_AutoAlign.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.688, + "y": 5.064 + }, + "prevControl": null, + "nextControl": { + "x": 7.352672365601742, + "y": 5.044277550428629 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.65848888888889, + "y": 7.328622222222221 + }, + "prevControl": { + "x": 5.980055555555556, + "y": 7.3383666666666665 + }, + "nextControl": { + "x": 4.79189997177251, + "y": 7.302361952006571 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8804131571116316, + "y": 7.3260829542246855 + }, + "prevControl": { + "x": 4.336156564442071, + "y": 7.314222453115628 + }, + "nextControl": { + "x": 3.4246697497811924, + "y": 7.337943455333743 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.369971469329529, + "y": 4.015592011412268 + }, + "prevControl": { + "x": 2.1111982881597715, + "y": 4.028530670470756 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -92.12109639666147 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NT_Turn_Back.path b/src/main/deploy/pathplanner/paths/NT_Turn_Back.path new file mode 100644 index 0000000..ab65a21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NT_Turn_Back.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.688, + "y": 5.064 + }, + "prevControl": null, + "nextControl": { + "x": 7.352672365601742, + "y": 5.044277550428629 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.811654778887304, + "y": 7.328622222222221 + }, + "prevControl": { + "x": 6.088362476926048, + "y": 7.324726384094327 + }, + "nextControl": { + "x": 4.944764621968615, + "y": 7.340827389443652 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3490555555555557, + "y": 7.328622222222221 + }, + "prevControl": { + "x": 3.807044444444444, + "y": 7.348111111111111 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.87821742568735 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -92.12109639666147 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Set_AutoAlign.path b/src/main/deploy/pathplanner/paths/Set_AutoAlign.path index c676dd4..c64dc07 100644 --- a/src/main/deploy/pathplanner/paths/Set_AutoAlign.path +++ b/src/main/deploy/pathplanner/paths/Set_AutoAlign.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5987333333333336, - "y": 4.0447444444444445 + "x": 1.9688730385164046, + "y": 4.00265335235378 }, "prevControl": { - "x": 1.634033333333334, - "y": 4.054488888888889 + "x": 1.5289586305278173, + "y": 3.937960057061342 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Trench_Neutral.path b/src/main/deploy/pathplanner/paths/Trench_Neutral.path new file mode 100644 index 0000000..da9d3d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Trench_Neutral.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8061626248216838, + "y": 7.327888730385164 + }, + "prevControl": null, + "nextControl": { + "x": 4.043333378719885, + "y": 7.248831577037986 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.199814550641939, + "y": 7.327888730385164 + }, + "prevControl": { + "x": 5.552881597717546, + "y": 7.405520684736091 + }, + "nextControl": { + "x": 6.5980558925497395, + "y": 7.2800997693562275 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.390171184022824, + "y": 6.706833095577747 + }, + "prevControl": { + "x": 7.221968616262481, + "y": 6.952667617689016 + }, + "nextControl": { + "x": 7.531342162920822, + "y": 6.50050628026529 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.688, + "y": 5.064 + }, + "prevControl": { + "x": 7.66188302425107, + "y": 5.361212553495007 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.9018768893355577 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java deleted file mode 100644 index cf59cea..0000000 --- a/src/main/java/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026-dango"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 166; - public static final String GIT_SHA = "51361bc2279a4d2a3f4c1af92a866ec921cdf29f"; - public static final String GIT_DATE = "2026-03-15 20:50:43 EDT"; - public static final String GIT_BRANCH = "Main"; - public static final String BUILD_DATE = "2026-03-15 21:10:38 EDT"; - public static final long BUILD_UNIX_TIME = 1773623438127L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2746f93..17069ba 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,18 +39,18 @@ public Robot() { // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - Logger.recordMetadata( - "GitDirty", - switch (BuildConstants.DIRTY) { - case 0 -> "All changes committed"; - case 1 -> "Uncommitted changes"; - default -> "Unknown"; - }); + // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + // Logger.recordMetadata( + // "GitDirty", + // switch (BuildConstants.DIRTY) { + // case 0 -> "All changes committed"; + // case 1 -> "Uncommitted changes"; + // default -> "Unknown"; + // }); // Set up data receivers & replay source switch (Constants.currentMode) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aad39ff..13f12e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,12 +7,12 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.generated.Constants; import frc.robot.commands.AutoAlign; import frc.robot.commands.AutoCommands; import frc.robot.commands.StateMachine; @@ -28,9 +28,9 @@ import frc.robot.subsystems.shooter.ShooterIOKraken; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.TeleopSwerve; -import frc.robot.subsystems.vision.limelight_vision.Vision; /** * This class is where the bulk of the robot should be declared. Since @@ -67,7 +67,6 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - vision = new Vision(swerve); if (Robot.isReal()) { shooter = new ShooterSubsystem(new ShooterIOKraken()); @@ -81,9 +80,10 @@ public RobotContainer() { hopper = new Hopper(new HopperIOSim()); } hoodedShooter = new HoodedShooter(); + vision = new Vision(); this.stateMachine = new StateMachine(shooter, hopper,intake); - this.autos = new AutoCommands(stateMachine, intake, shooter, swerve, vision); + this.autos = new AutoCommands(stateMachine, intake, shooter, hoodedShooter, swerve, vision); // Configure the button bindings configureButtonBindings(); @@ -98,36 +98,6 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // // Default command, normal field-relative drive - // swerve.setDefaultCommand( - // DriveCommands.joystickDrive( - // swerve, - // () -> -driverController.getLeftY(), - // () -> -driverController.getLeftX(), - // () -> -driverController.getRightX() - // ) - // ); - - // // Lock to 0° when A button is held - // driverController - // .a() - // .whileTrue( - // DriveCommands.joystickDriveAtAngle( - // swerve, - // () -> -driverController.getLeftY(), - // () -> -driverController.getLeftX(), - // () -> Rotation2d.kZero - // ) - // ); - - // // Switch to X pattern when X button is pressed - // driverController.x().onTrue(Commands.runOnce(swerve::stopWithX, swerve)); - - // // Reset gyro to 0° when Y button is pressed - // driverController.y().onTrue(Commands.runOnce(() ->swerve.setPose( - // new Pose2d(swerve.getPose().getTranslation(), - // Rotation2d.kZero)),swerve).ignoringDisable(true)); - swerve.setDefaultCommand(new TeleopSwerve( swerve, () -> -driverController.getLeftY(), @@ -136,84 +106,42 @@ private void configureButtonBindings() { () -> driverController.back().getAsBoolean())); // allows you to drive as robot relative // only while holding down the button - driverController.y().onTrue(Commands.runOnce(() -> swerve.resetGyro())); - - // Intake & Shooter - // driverController.rightTrigger().and(driverController.rightBumper()).onTrue( - // stateMachine.changeState(RobotState.SHOOT_ONLY)); - // driverController.rightTrigger().negate().and(driverController.rightBumper()).onTrue( - // stateMachine.changeState(RobotState.IDLE)); - // driverController.rightBumper().negate().and(driverController.rightTrigger()).onTrue( - // stateMachine.changeState(RobotState.SHOOT_ONLY)); - // driverController.rightBumper().negate().and(driverController.rightTrigger().negate()).onTrue( - // stateMachine.changeState(RobotState.IDLE)); + driverController.y().onTrue(Commands.runOnce(() -> { + swerve.resetGyro(); + vision.resetOffset(); + })); - driverController.rightTrigger().onTrue(stateMachine.changeState(RobotState.SHOOT_ONLY)).onFalse(stateMachine.changeState(RobotState.IDLE));//.onFalse(stateMachine.changeState(RobotState.INTAKE_DOWN)); + driverController.rightTrigger().onTrue(stateMachine.changeState(RobotState.SHOOT_ONLY)).onFalse(stateMachine.changeState(RobotState.IDLE)); driverController.rightBumper().onTrue( Commands.either( - intake.changeState(IntakeState.IDLE), - intake.changeState(IntakeState.DEPLOYED), + Commands.runOnce(()->intake.setWantedState(IntakeState.IDLE)), + Commands.runOnce(()->intake.setWantedState(IntakeState.DEPLOYED)), () -> intake.getState() == IntakeState.DEPLOYED)); - // Intake & Shooter - // driverController.rightTrigger().and(driverController.rightBumper()).onTrue( - // intakeDown?stateMachine.changeState(RobotState.SHOOT_ONLY) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown)): - // stateMachine.changeState(RobotState.INTAKE_DOWN_AND_SHOOT) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown))); - - // driverController.rightTrigger().negate().and(driverController.rightBumper()).onTrue( - // intakeDown?stateMachine.changeState(RobotState.IDLE) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown)): - // stateMachine.changeState(RobotState.INTAKE_DOWN) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown))); - - // driverController.rightBumper().negate().and(driverController.rightTrigger()).onTrue( - // intakeDown?stateMachine.changeState(RobotState.INTAKE_DOWN_AND_SHOOT) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown)): - // stateMachine.changeState(RobotState.SHOOT_ONLY) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown))); - - // driverController.rightBumper().negate().and(driverController.rightTrigger().negate()).onTrue( - // intakeDown?stateMachine.changeState(RobotState.INTAKE_DOWN) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown)): - // stateMachine.changeState(RobotState.IDLE) - // .andThen(Commands.runOnce(()->intakeDown=!intakeDown))); - driverController.leftBumper().onTrue( - Commands.parallel(shooter.runFeederBack(), hopper.runHopperBack()) + Commands.parallel( + Commands.runOnce(() -> intake.setWantedState(IntakeState.WIGGLING))) ).onFalse( Commands.either( Commands.parallel(shooter.runFeeder(), hopper.runHopper()), Commands.parallel(shooter.stopFeeder(), hopper.stopHopper()), () -> stateMachine.getCurrentState() == RobotState.SHOOT_ONLY || stateMachine.getCurrentState() == RobotState.INTAKE_DOWN_AND_SHOOT)); - - driverController.povDown().onTrue(Commands.runOnce(() -> { - hoodedShooter.moveHood(-0.05); - })).onFalse(Commands.runOnce(() -> { - hoodedShooter.moveHood(0);})); - driverController.povUp().onTrue(Commands.runOnce(() -> { - hoodedShooter.moveHood(0.05); - })).onFalse(Commands.runOnce(() -> { - hoodedShooter.moveHood(0); - })); - ; + + // D-pad: step hood ±5° using MotionMagic position hold + driverController.povDown().onTrue(Commands.runOnce(() -> hoodedShooter.stepHood(-Constants.HoodedShooterConstants.hoodStepDegrees), hoodedShooter)); + driverController.povUp().onTrue( Commands.runOnce(() -> hoodedShooter.stepHood( Constants.HoodedShooterConstants.hoodStepDegrees), hoodedShooter)); driverController.leftTrigger().whileTrue(new AutoAlign( - swerve, - vision, - () -> DriverStation.getAlliance().isPresent() && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red + swerve, vision, shooter, hoodedShooter, + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> -driverController.getRightX() )); - - // operatorController.a().onTrue(Commands.runOnce(() -> shooter.setTargetRPM("hub"), shooter)); - // operatorController.b().onTrue(Commands.runOnce(() -> shooter.setTargetRPM("default"), shooter)); - // operatorController.x().onTrue(Commands.runOnce(() -> shooter.setTargetRPM("outpost"), shooter)); - // operatorController.y().onTrue(Commands.runOnce(() -> shooter.setTargetRPM("trench"), shooter)); - - // operatorController.a().or(operatorController.b()).or(operatorController.x()).or(operatorController.y()) - // .onFalse(Commands.runOnce(() -> shooter.setTargetRPM("default"), shooter)); + + // bind to copilot D-pad + operatorController.povUp().onTrue(Commands.runOnce(() -> vision.adjustOffset(100.0))); + operatorController.povDown().onTrue(Commands.runOnce(() -> vision.adjustOffset(-100.0))); } public Command getAutonomousCommand() { @@ -223,4 +151,5 @@ public Command getAutonomousCommand() { public void resetModulesToAbsolute() { swerve.resetModulesToAbsolute(); } + } diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java index 21f85c3..f41af05 100644 --- a/src/main/java/frc/robot/commands/AutoAlign.java +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -1,88 +1,103 @@ package frc.robot.commands; -import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.generated.Constants; -// import frc.robot.subsystems.shooter.*; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.vision.limelight_vision.Vision; +import frc.robot.subsystems.shooter.HoodedShooter; +import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.vision.Vision; +/** + * Points the robot at the hub using raw Limelight tx, and commands + * shooter RPM + hood angle from the ty-based distance lookup. + * + * Driver keeps full translation control; rotation is overridden by the PID. + */ public class AutoAlign extends Command { + private final Swerve swerve; private final Vision vision; - // private final ShooterSubsystem shooter; - private final BooleanSupplier isRedSupplier; - private final PIDController rotationPID; - private final PIDController distancePID; - private boolean isRed; + private final ShooterSubsystem shooter; + private final HoodedShooter hoodedShooter; + private final DoubleSupplier xTranslation; + private final DoubleSupplier yTranslation; + private final DoubleSupplier driverRotation; + private final PIDController txPID; - public AutoAlign(Swerve swerve, Vision vision, /*ShooterSubsystem shooter,*/ BooleanSupplier isRedSupplier) { + public AutoAlign(Swerve swerve, Vision vision, ShooterSubsystem shooter, HoodedShooter hoodedShooter, + DoubleSupplier xTranslation, DoubleSupplier yTranslation, DoubleSupplier driverRotation) { this.swerve = swerve; this.vision = vision; - // this.shooter = shooter; - this.isRedSupplier = isRedSupplier; - rotationPID = Constants.Vision.rotationPID; - rotationPID.setTolerance(2.0); - rotationPID.enableContinuousInput(-180, 180); - distancePID = Constants.Vision.distancePID; - distancePID.setTolerance(0.1); - addRequirements(this.swerve); + this.shooter = shooter; + this.hoodedShooter = hoodedShooter; + this.xTranslation = xTranslation; + this.yTranslation = yTranslation; + this.driverRotation = driverRotation; + addRequirements(swerve, shooter, hoodedShooter); + + // PID on shooter error (degrees). 0 = shooter axis aligned with target. + txPID = new PIDController( + Constants.Vision.rotationPID.getP(), + Constants.Vision.rotationPID.getI(), + Constants.Vision.rotationPID.getD()); + txPID.setSetpoint(0); + txPID.setTolerance(2.0); } @Override public void initialize() { - // Evaluate alliance now, when FMS is actually connected - isRed = isRedSupplier.getAsBoolean(); - rotationPID.reset(); - distancePID.reset(); + txPID.reset(); } @Override public void execute() { - if (!vision.hasHubTarget(isRed)) { - swerve.drive(new Translation2d(0, 0), 0, true, true); - return; - } + // Driver translation (same deadband + cubic + maxSpeed as TeleopSwerve). + double x = MathUtil.applyDeadband(xTranslation.getAsDouble(), Constants.stickDeadband); + double y = MathUtil.applyDeadband(yTranslation.getAsDouble(), Constants.stickDeadband); + Translation2d raw = new Translation2d(x, y); + double magnitude = raw.getNorm(); + Translation2d driverInput = raw.times(Math.pow(magnitude, 2)).times(Constants.Swerve.maxSpeed); - Rotation2d targetHeading = vision.getHeadingToScorePillar(isRed); - double distance = vision.getDistanceToScorePillar(isRed); - // shooter.setTargetRPM(distance); // TODO: enable variable RPM once tuned + if (vision.hasTarget()) { + // Rotate to zero out the shooter-to-target error. + double error = vision.getShooterErrorDeg(); + double pidOutput = txPID.calculate(error); + double rotationSpeed = MathUtil.clamp(pidOutput, + -Constants.Swerve.maxAngularVelocity, Constants.Swerve.maxAngularVelocity); - double rotation = rotationPID.calculate( - swerve.getHeading().getDegrees(), - targetHeading.getDegrees() - ); - rotation = MathUtil.clamp(rotation, -Constants.Swerve.maxAngularVelocity, Constants.Swerve.maxAngularVelocity); + // Command shooter RPM and hood angle from distance-based LUT. + shooter.commandRPM(vision.getTargetRPM()); + hoodedShooter.moveHoodToAngleWithOffset(vision.getTargetHoodAngleDeg()); - Translation2d translation = new Translation2d(0, 0); - if (!Double.isNaN(distance)) { - double translationSpeed = -distancePID.calculate(distance, Constants.Vision.targetDistanceMeters); - translationSpeed = MathUtil.clamp(translationSpeed, -Constants.Swerve.maxSpeed, Constants.Swerve.maxSpeed); - // targetHeading has +PI for rear launcher, so subtract PI to get the direction toward the tag - Rotation2d directionToTag = targetHeading.minus(new Rotation2d(Math.PI)); - translation = new Translation2d(translationSpeed, directionToTag); - } - swerve.drive(translation, rotation, true, true); + swerve.drive(driverInput, rotationSpeed, true, true); - SmartDashboard.putNumber("Vision/Distance", Double.isNaN(distance) ? -1 : distance); - SmartDashboard.putNumber("Vision/TargetHeading", targetHeading.getDegrees()); - SmartDashboard.putNumber("Vision/CurrentHeading", swerve.getHeading().getDegrees()); + SmartDashboard.putNumber("AutoAlign/ShooterErrorDeg", error); + SmartDashboard.putNumber("AutoAlign/DistanceM", vision.getDistanceM()); + SmartDashboard.putNumber("AutoAlign/RPM", vision.getTargetRPM()); + SmartDashboard.putNumber("AutoAlign/HoodAngleDeg", vision.getTargetHoodAngleDeg()); + SmartDashboard.putNumber("AutoAlign/RotationPID", pidOutput); + } else { + // No target — give driver full rotation control, keep shooter warm. + shooter.setTargetRPM(4500); + double rotation = MathUtil.applyDeadband(driverRotation.getAsDouble(), Constants.stickDeadband); + rotation = Math.pow(rotation, 3) * Constants.Swerve.maxAngularVelocity; + swerve.drive(driverInput, rotation, true, true); + } } @Override public boolean isFinished() { - return rotationPID.atSetpoint() && distancePID.atSetpoint(); + return false; // runs until trigger is released } @Override public void end(boolean interrupted) { - // shooter.setTargetRPM(Constants.Shooter.TARGET_RPM_DEFAULT); - swerve.drive(new Translation2d(0, 0), 0, true, true); + swerve.drive(new Translation2d(), 0, true, true); } } diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 2dfeac2..ae106fb 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -6,7 +6,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -14,23 +13,23 @@ import frc.robot.commands.StateMachine.RobotState; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.HoodedShooter; import frc.robot.subsystems.shooter.ShooterSubsystem; -import frc.robot.subsystems.vision.limelight_vision.Vision; +import frc.robot.subsystems.vision.Vision; public class AutoCommands { private final SendableChooser autoChooser=new SendableChooser();; private final StringPublisher selectedAuto; private final NetworkTable autoNetworkTable; - public AutoCommands(StateMachine stateMachine, Intake intake, ShooterSubsystem shooter, Swerve swerve, Vision vision){ + public AutoCommands(StateMachine stateMachine, Intake intake, ShooterSubsystem shooter, HoodedShooter hoodedShooter, Swerve swerve, Vision vision){ autoNetworkTable = NetworkTableInstance.getDefault().getTable("Auto"); selectedAuto = autoNetworkTable.getStringTopic("selectedAuto").publish(); selectedAuto.set("Nothing"); NamedCommands.registerCommand("AutoAlign", - new AutoAlign(swerve, vision, - () -> DriverStation.getAlliance().isPresent() && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red)); + new AutoAlign(swerve, vision, shooter, hoodedShooter, + () -> 0.0, () -> 0.0, () -> 0.0)); NamedCommands.registerCommand("Shoot", new InstantCommand(() -> stateMachine.scheduleNewState(RobotState.SHOOT_ONLY))); @@ -43,14 +42,10 @@ public AutoCommands(StateMachine stateMachine, Intake intake, ShooterSubsystem s autoChooser.setDefaultOption("Nothing", new InstantCommand()); autoChooser.addOption("Test_Auto", new PathPlannerAuto("Test_Auto")); - autoChooser.addOption("B1_Hub_HP", new PathPlannerAuto("B1_Hub_HP")); - autoChooser.addOption("B2_Hub_HP", new PathPlannerAuto("B2_Hub_HP")); - autoChooser.addOption("B3_Hub_HP", new PathPlannerAuto("B3_Hub_HP")); + autoChooser.addOption("N_Shoot", new PathPlannerAuto("N_Shoot")); + autoChooser.addOption("Ideal_N_Shoot", new PathPlannerAuto("Ideal_N_Shoot")); autoChooser.addOption("Shoot_Still", new PathPlannerAuto("Shoot_Still")); - autoChooser.addOption("B1_Hub_HP_Shoot", new PathPlannerAuto("B1_Hub_HP_Shoot")); - autoChooser.addOption("B2_Hub_HP_Shoot", new PathPlannerAuto("B2_Hub_HP_Shoot")); autoChooser.addOption("AutoAlign_Shoot", new PathPlannerAuto("AutoAlign_Shoot")); - autoChooser.addOption("B3_Hub_HP_Shoot", new PathPlannerAuto("B3_Hub_HP_Shoot")); autoChooser.addOption("Shoot_N_Shoot", new PathPlannerAuto("Shoot_N_Shoot")); autoChooser.addOption("Shoot_Neutral_Trench", new PathPlannerAuto("Shoot_Neutral_Trench")); SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/StateMachine.java b/src/main/java/frc/robot/commands/StateMachine.java index 5a9aa83..80dc7a3 100644 --- a/src/main/java/frc/robot/commands/StateMachine.java +++ b/src/main/java/frc/robot/commands/StateMachine.java @@ -18,17 +18,17 @@ public enum RobotState { IDLE(ShooterState.IDLE,HopperState.IDLE,IntakeState.IDLE), - SHOOT_ONLY(ShooterState.SHOOT,HopperState.RUNNING, IntakeState.IDLE),//IntakeState.DEPLOYED + SHOOT_ONLY(ShooterState.SHOOT_INIT,HopperState.RUNNING, IntakeState.WIGGLING),//IntakeState.DEPLOYED INTAKE_DOWN(ShooterState.IDLE,HopperState.IDLE,IntakeState.DEPLOYED), //WIGGLING(IntakeState.WIGGLING,ShooterState.IDLE,HopperState.IDLE), //INTAKE_DOWN_SHOOT(IntakeState.DEPLOYED, ShooterState.SHOOT,HopperState.RUNNING), //INTAKE_ROLL_IN(IntakeState.ROLLERS_IN,ShooterState.IDLE,HopperState.IDLE), - INTAKE_DOWN_AND_SHOOT(ShooterState.SHOOT,HopperState.RUNNING, IntakeState.DEPLOYED); + INTAKE_DOWN_AND_SHOOT(ShooterState.PRESHOOT,HopperState.RUNNING, IntakeState.WIGGLING); //INTAKE_ROLL_OUT(IntakeState.ROLLERS_OUT,ShooterState.IDLE,HopperState.IDLE); //INTAKE_ROLL_OUT_AND_SHOOT(IntakeState.ROLLERS_OUT,ShooterState.SHOOT,HopperState.RUNNING), //INTAKE_WIGGLE_AND_SHOOT(IntakeState.WIGGLING,ShooterState.SHOOT,HopperState.RUNNING); - + public final ShooterState shooterState; public final HopperState hopperState; public final IntakeState intakeState; @@ -56,7 +56,7 @@ public StateMachine(ShooterSubsystem shooter, Hopper hopper, Intake intake) { this.intake = intake; this.state = RobotState.IDLE; - + this.stateTable = NetworkTableInstance.getDefault().getTable("StateMachine"); this.currentStatePub = stateTable.getStringTopic("CurrentState").publish(); } @@ -79,15 +79,15 @@ public RobotState getState(){ //TODO: Combine public Command changeState(RobotState newState) { - + return Commands.sequence( - Commands.runOnce(() -> + Commands.runOnce(() -> state = newState ), Commands.parallel( shooter.changeState(newState.shooterState), hopper.changeState(newState.hopperState), - intake.changeState(newState.intakeState)) + Commands.runOnce(() -> intake.setWantedState(newState.intakeState))) ); } @@ -98,4 +98,4 @@ public RobotState getCurrentState() { private void publishStates() { currentStatePub.set(state.toString()); } - } \ No newline at end of file + } diff --git a/src/main/java/frc/robot/generated/Constants.java b/src/main/java/frc/robot/generated/Constants.java index cbea14b..ba82a03 100644 --- a/src/main/java/frc/robot/generated/Constants.java +++ b/src/main/java/frc/robot/generated/Constants.java @@ -17,10 +17,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; @@ -31,6 +28,7 @@ import frc.lib.util.COTSTalonFXSwerveConstants; import frc.lib.util.SwerveModuleConstants; import frc.robot.subsystems.SwerveModule; + import frc.robot.Robot; @@ -44,25 +42,12 @@ public class Constants { public static final class Vision { public static final String primaryLimelightName = "limelight-left"; public static final String secondaryLimelightName = "limelight-right"; - public static final double ERROR_DEGREES = 5.0; //TODO: Tune this if needed - public static final int[] RED_HUB_TAGS = {5, 8, 9, 10, 11, 2}; - public static final int[] BLUE_HUB_TAGS = {18, 27, 21, 24, 25, 26}; - public static PIDController rotationPID = new PIDController(0.12, 0, 0.0); - public static PIDController distancePID = new PIDController(1.3, 0, 0); - public static final double targetDistanceMeters = Units.feetToMeters(9); - // limelight-left (primary): forward=-0.263525m, right=-0.263525m, up=0.2439162m, roll=0°, pitch=20°, yaw=150° - // Y is negated because pose3dToArray outputs WPILib Y (left) but Limelight interprets it as right - public static Pose3d cameraPosePrimary = new Pose3d( - new Translation3d(-0.263525, 0.263525, 0.2439162), - new Rotation3d(0, Math.toRadians(20), Math.toRadians(150)) - ); - // limelight-right (secondary): forward=-0.263525m, right=0.263525m, up=0.2439162m, roll=0°, pitch=20°, yaw=-150° - public static Pose3d cameraPoseSecondary = new Pose3d( - new Translation3d(-0.263525, -0.263525, 0.2439162), - new Rotation3d(0, Math.toRadians(20), Math.toRadians(-150)) - ); + public static PIDController rotationPID = new PIDController(0.1, 0, 0.0); } public static final class Shooter{ + // 3T motor pulley : 4T flywheel pulley — flywheel spins 4/3 faster than motor. + // All RPM values in this codebase are FLYWHEEL RPM. ShooterIOKraken applies this ratio internally. + public static final double FLYWHEEL_GEAR_RATIO = 3.0 / 4.0; // motor rotations per flywheel rotation public static final double TARGET_RPM_DEFAULT = 4500; public static double SHOOTER_KS = 0.0; public static double SHOOTER_KV = 0.12; //0.12 @@ -290,7 +275,7 @@ public static final class IntakeConstants { public static final int rightPivotMotorId = 6; public static final int rollerMotorId = 10; - public static final double intakeAngleDeg = 135; + public static final double intakeAngleDeg = 145; public static final double angleToleranceDeg = 5.0; public static final double stowedAngleDeg = 0; @@ -304,6 +289,8 @@ public static final class IntakeConstants { public static final double pivotD = 0.0; public static final double rollerSpeed = -0.35;//0.60; + public static final double HIGH_WIGGLE_POSITION_DEGREES = 80; + public static final double LOW_WIGGLE_POSITION_DEGREES = 100; // public static final double wiggleLowDeg = 90.0; // public static final double wiggleHighDeg = 110.0; @@ -320,12 +307,19 @@ public static enum Mode { } public static final class HoodedShooterConstants{ - public static final double cruiseVelocityRps = .25; - public static final double accelRps2 =.125; + public static final double cruiseVelocityRps = 5.0; // motor rps → ~97°/s hood + public static final double accelRps2 = 20.0; - public static final double hoodP = 0.8; + public static final double hoodP = 5.0; public static final double hoodI = 0.0; public static final double hoodD = 0.0; + + public static final double hoodStepDegrees = 5.0; + public static final double hoodMinDegrees = 5.0; // mechanical limit (degrees) + public static final double hoodMaxDegrees = 35.0; // mechanical limit (degrees) + + // Full gear chain: (24/18) * (167/12) = 18.556 motor rotations per hood rotation + public static final double motorRotationsPerHoodRotation = (24.0 / 18.0) * (167.0 / 12.0); } public static final class AutoConstants { //TODO: Need to tune constants! diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index fb89411..10ab8fe 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -8,7 +8,7 @@ import static edu.wpi.first.units.Units.Volts; -import org.photonvision.EstimatedRobotPose; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.StatusSignal; @@ -37,7 +37,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -62,6 +61,11 @@ public class Swerve extends SubsystemBase { // private final HttpCamera camStream; + // Offset between raw gyro and field heading, updated on every pose reset. + // fieldHeading = rawGyro + gyroOffset. This avoids both the CAN race + // condition of setYaw() AND the feedback loop of using estimator heading. + private Rotation2d gyroOffset = Rotation2d.kZero; + private double simCurrentDrawAmps = 0; private final DoubleEntry xPosEntry; private long xPosEntryLastChanged; @@ -84,9 +88,12 @@ public Swerve() { new SwerveModule(2, Constants.Swerve.Mod2.constants), //Back Left Module new SwerveModule(3, Constants.Swerve.Mod3.constants) //Back Right Module }; + // Use the target yaw (0°) directly — gyro.setYaw(0) above is async (CAN bus), + // so getGyroYaw() still returns the stale pre-reset value. Using it would give + // the estimator a wrong internal offset, making the initial heading incorrect. poseEstimator = new SwerveDrivePoseEstimator( Constants.Swerve.swerveKinematics, - getGyroYaw(), + Rotation2d.fromDegrees(0), getModulePositions(), new Pose2d()); @@ -133,16 +140,6 @@ public Swerve() { yPosEntry = null; rotEntry = null; } - // gyroDoublePublisher = table.getDoubleTopic("GyroYaw").publish(); - // cancoderPubs = new DoublePublisher[4]; - // anglePubs = new DoublePublisher[4]; - // velocityPubs = new DoublePublisher[4]; - - for (int i = 0; i < 4; i++) { - cancoderPubs[i] = table.getDoubleTopic("Module " + i + "/CANcoder").publish(); - anglePubs[i] = table.getDoubleTopic("Module " + i + "/Angle").publish(); - velocityPubs[i] = table.getDoubleTopic("Module " + i + "/Velocity").publish(); - } driveSysIdRoutine = new SysIdRoutine( new SysIdRoutine.Config( null, // Use default ramp rate (1 V/s) @@ -261,9 +258,14 @@ public static enum AlignmentPosition { RIGHT } - private ChassisSpeeds getRobotRelativeSpeeds() { + public ChassisSpeeds getRobotRelativeSpeeds() { return Constants.Swerve.swerveKinematics.toChassisSpeeds(getModuleStates()); } + + public ChassisSpeeds getFieldVelocity() { // Added this new method + // Uses your existing methods to get the robot speeds and the gyro heading + return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getHeading()); + } private void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { SwerveModuleState[] states = Constants.Swerve.swerveKinematics.toSwerveModuleStates(robotRelativeSpeeds); @@ -315,10 +317,14 @@ public SwerveModuleState[] getModuleStates() { public Command resetHeading() { return runOnce(() -> { - setPose( - new Pose2d( - getPose().getTranslation(), - AllianceUtil.isRedAlliance() ? new Rotation2d(Math.PI) : new Rotation2d())); + // Don't call gyro.setYaw() — it's async over CAN and creates a race condition. + // Just tell the estimator "the gyro currently reads X, and I want heading Y". + // The estimator computes the offset internally. + Rotation2d rawYaw = getGyroYaw(); + Rotation2d targetYaw = Rotation2d.fromDegrees(AllianceUtil.isRedAlliance() ? 180.0 : 0.0); + gyroOffset = targetYaw.minus(rawYaw); + poseEstimator.resetPosition(rawYaw, getModulePositions(), + new Pose2d(getPose().getTranslation(), targetYaw)); }); } @@ -336,7 +342,13 @@ public Pose2d getPose() { } public void setPose(Pose2d pose) { - poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); + // Refresh the gyro signal so we read the latest CAN value, not a stale cache. + // A stale reading here produces a wrong internal offset in the estimator, + // causing odometry to drift until vision corrects it. + gyroYaw.refresh(); + Rotation2d rawYaw = getGyroYaw(); + gyroOffset = pose.getRotation().minus(rawYaw); + poseEstimator.resetPosition(rawYaw, getModulePositions(), pose); } public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -345,13 +357,22 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS } public Rotation2d getHeading() { - return getGyroYaw();// getPose().getRotation(); + return getPose().getRotation(); } public Rotation2d getGyroYaw() { return Rotation2d.fromDegrees(gyroYaw.getValueAsDouble()); } + /** + * Returns the field-relative heading derived purely from the gyro, with + * the offset applied from the last pose reset. Unlike getHeading(), this + * is never influenced by vision corrections — safe for SetRobotOrientation. + */ + public Rotation2d getFieldHeadingFromGyro() { + return getGyroYaw().plus(gyroOffset); + } + public void resetModulesToAbsolute() { for (SwerveModule mod : mSwerveMods) { if (Math.abs(mod.getCANcoderWithOffset().getDegrees() - mod.getState().angle.getDegrees()) > 10) @@ -361,35 +382,28 @@ public void resetModulesToAbsolute() { public Command resetPositionToFrontReef() { Waypoint bluePoint = new Waypoint(null, new Translation2d(3.171, 4.024), null); - return Commands.sequence( - runOnce(() -> { - setPose(AllianceUtil.isRedAlliance() ? new Pose2d(bluePoint.flip().anchor(), new Rotation2d(180.0)) - : new Pose2d(bluePoint.anchor(), new Rotation2d(0.0))); - resetGyro(); - })); - - } - - public void resetGyro() { - // if (AllianceUtil.isRedAlliance()) gyro.setYaw(180); - // else - gyro.setYaw(0); + return runOnce(() -> { + Rotation2d rawYaw = getGyroYaw(); + Pose2d targetPose = AllianceUtil.isRedAlliance() + ? new Pose2d(bluePoint.flip().anchor(), Rotation2d.fromDegrees(180)) + : new Pose2d(bluePoint.anchor(), new Rotation2d(0.0)); + gyroOffset = targetPose.getRotation().minus(rawYaw); + poseEstimator.resetPosition(rawYaw, getModulePositions(), targetPose); + }); } /** - * The latest estimated robot pose on the field from vision data. This may be - * empty. This should - * only be called once per loop. - * - *

- * Also includes updates for the standard deviations, which can (optionally) be - * retrieved with - * {@link getEstimationStdDevs} - * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate - * timestamp, and targets - * used for estimation. + * Resets the estimator heading to 0° (Blue) or 180° (Red) without touching + * the hardware gyro. The estimator computes an internal offset from the + * current raw gyro reading, so there is no CAN race condition. */ + public void resetGyro() { + Rotation2d rawYaw = getGyroYaw(); + Rotation2d targetYaw = Rotation2d.fromDegrees(AllianceUtil.isRedAlliance() ? 180.0 : 0.0); + gyroOffset = targetYaw.minus(rawYaw); + poseEstimator.resetPosition(rawYaw, getModulePositions(), + new Pose2d(getPose().getTranslation(), targetYaw)); + } @Override public void periodic() { @@ -406,7 +420,6 @@ public void periodic() { updateOdom(); Pose2d currentPose = getPose(); - currentPose = getPose(); field.setRobotPose(currentPose); gyroDoublePublisher.set(getGyroYaw().getDegrees()); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 52eaefd..a5f9dde 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,18 +1,22 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.generated.Constants; import frc.robot.generated.Constants.IntakeConstants; public class Intake extends SubsystemBase{ private IntakeIO io; - private IntakeState state; + private boolean wiggleUp = true; + + public enum IntakeState { IDLE(false, 0,IntakeConstants.stowedAngleDeg), - DEPLOYED(true, 0.35,IntakeConstants.intakeAngleDeg), + DEPLOYED(true, 0.5,IntakeConstants.intakeAngleDeg), WIGGLING(true, 0,80); public final boolean intakeExtended; @@ -25,42 +29,117 @@ private IntakeState(boolean extended, double rollerSpeed, double pivotAngle) { this.pivotAngle = pivotAngle; } } + + public enum IntakePivotState { + IDLE, + MOVING_TO_SETPOINT, + AT_SETPOINT, + SWITCHING_AGITATE_TARGET_HIGH, + SWITCHING_AGITATE_TARGET_LOW + } + private IntakeState wantedState = IntakeState.IDLE; + private IntakePivotState currentState = IntakePivotState.IDLE; + private IntakePivotState previousState = IntakePivotState.IDLE; public Intake(IntakeIO io){ io.zeroPivot(); this.io = io; - this.state = IntakeState.IDLE; - io.setState(IntakeState.IDLE); + io.setState(IntakeState.IDLE.pivotAngle); } public boolean intakeAtTargetPos(){ return (io.isPivotAtTarget()); } public IntakeState getState(){ - return this.state; + return this.wantedState; + } + + public void setWantedState(IntakeState state) { + this.wantedState = state; } - public Command changeState(IntakeState newState) { - return Commands.runOnce(()->{ - this.state = newState; - if(newState == IntakeState.DEPLOYED){ - io.setStateRollers(newState.rollerSpeed*-1); + public void changeState() { + previousState = this.currentState; + if (this.wantedState == IntakeState.WIGGLING) { + double target = (wiggleUp)? Constants.IntakeConstants.HIGH_WIGGLE_POSITION_DEGREES : Constants.IntakeConstants.LOW_WIGGLE_POSITION_DEGREES; + boolean atTarget = io.isPivotAtSetpoint(target); + if (previousState == IntakePivotState.MOVING_TO_SETPOINT && atTarget) { + currentState = + wiggleUp + ? IntakePivotState.SWITCHING_AGITATE_TARGET_LOW + : IntakePivotState.SWITCHING_AGITATE_TARGET_HIGH; + } else if (currentState == IntakePivotState.SWITCHING_AGITATE_TARGET_HIGH) { + wiggleUp = true; + currentState = IntakePivotState.AT_SETPOINT; + } else if (currentState == IntakePivotState.SWITCHING_AGITATE_TARGET_LOW) { + wiggleUp = false; + currentState = IntakePivotState.AT_SETPOINT; + } else { + currentState = + atTarget + ? IntakePivotState.AT_SETPOINT + : IntakePivotState.MOVING_TO_SETPOINT; + } + }else if(wantedState == IntakeState.DEPLOYED){ + currentState = io.isPivotAtSetpoint(Constants.IntakeConstants.intakeAngleDeg) + ? IntakePivotState.AT_SETPOINT + : IntakePivotState.MOVING_TO_SETPOINT; }else{ - io.setStateRollers(0); + if (io.isPivotAtSetpoint(Constants.IntakeConstants.stowedAngleDeg)) { + currentState = IntakePivotState.AT_SETPOINT; + } else { + currentState = IntakePivotState.MOVING_TO_SETPOINT; + } } - io.setState(newState); - }, this).andThen(Commands.waitUntil(this::intakeAtTargetPos)) - .andThen(Commands.runOnce(()->io.setStateRollers(newState.rollerSpeed))); } + + private void applyState() { + switch (currentState) { + case MOVING_TO_SETPOINT: + io.setState(getTargetPos()); + if(wantedState==IntakeState.DEPLOYED){ + io.setStateRollers(wantedState.rollerSpeed*-1); + } + break; + case AT_SETPOINT: + io.setState(getTargetPos()); + if(wantedState==IntakeState.DEPLOYED){ + io.setStateRollers(wantedState.rollerSpeed); + } + break; + case SWITCHING_AGITATE_TARGET_HIGH: + io.setState(getTargetPos()); + break; + case SWITCHING_AGITATE_TARGET_LOW: + io.setState(getTargetPos()); + break; + case IDLE: + io.setStateRollers(0); + break; + default: + io.zeroPivot(); + break; + } + } + + private double getTargetPos() { + switch (wantedState) { + case WIGGLING: + return wiggleUp ? Constants.IntakeConstants.HIGH_WIGGLE_POSITION_DEGREES + : Constants.IntakeConstants.LOW_WIGGLE_POSITION_DEGREES; + case DEPLOYED: + return Constants.IntakeConstants.intakeAngleDeg; + default: + return Constants.IntakeConstants.stowedAngleDeg; + } +} @Override public void periodic(){ - // io.getMotorPos(); - // io.runPivotToTarget(); - // io.changeIfWiggle(io.isPivotAtTarget()); - // if (!manualRoll) { - // io.updateRollers(); - // } + if (DriverStation.isEnabled()) { + changeState(); + applyState(); + } SmartDashboard.putNumber("Intake/kP", IntakeConstants.pivotP); - SmartDashboard.putString("Intake/State", state.name()); + SmartDashboard.putString("Intake/State", wantedState.name()); SmartDashboard.putNumber("Intake/PivotDeg", io.getPivotAngle()); SmartDashboard.putNumber("Intake/PivotTargetDeg", io.getPivotTargetAngle()); SmartDashboard.putBoolean("Intake/PivotAtTarget", io.isPivotAtTarget()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 279d154..d3e08c8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -15,7 +15,8 @@ public interface IntakeIO { void zeroPivot(); void runRollers(); void stopRollers(); - void setState(IntakeState newState); + void setState(double pos); void getMotorPos(); void setStateRollers(double rollerSpeed); + boolean isPivotAtSetpoint(double targetDeg); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeReal.java index 1250523..5a8379e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeReal.java @@ -38,7 +38,7 @@ public IntakeReal() { configurePivot(); configureRoller(); - setState(IntakeState.IDLE); + setState(IntakeState.IDLE.pivotAngle); } // Apply Motion Magic + PID + current limit + brake mode for the pivot. @@ -79,11 +79,9 @@ public void configureRoller() { } // Set high-level state; updates pivot setpoint and roller behavior. - public void setState(IntakeState newState) { - this.state = newState; - pivotTargetDeg = newState.pivotAngle; + public void setState(double pos) { //leftPivotMotor.setControl(new DutyCycleOut(0)); - leftPivotMotor.setControl(pivotControl.withPosition(degreesToMotorRotations(pivotTargetDeg))); + leftPivotMotor.setControl(pivotControl.withPosition(degreesToMotorRotations(pos))); } @Override @@ -117,6 +115,10 @@ public boolean isPivotAtTarget() { return Math.abs(getPivotAngle() - pivotTargetDeg) <= IntakeConstants.angleToleranceDeg; } + public boolean isPivotAtSetpoint(double targetDeg) { + return Math.abs(getPivotAngle() - targetDeg) <= IntakeConstants.angleToleranceDeg; + } + public void runRollers(){ rollerMotor.set(IntakeConstants.rollerSpeed); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeSim.java index 5117c47..4f918f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSim.java @@ -39,7 +39,7 @@ public void configureRoller() { } @Override - public void setState(IntakeState newState) { + public void setState(double newState) { } @@ -87,5 +87,11 @@ public void setStateRollers(double rollerSpeed) { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'setStateRollers'"); } + + @Override + public boolean isPivotAtSetpoint(double targetDeg) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isPivotAtSetpoint'"); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodedShooter.java b/src/main/java/frc/robot/subsystems/shooter/HoodedShooter.java index 40c1568..26c064a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodedShooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodedShooter.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Degrees; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -9,11 +8,10 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.MathUtil; import frc.robot.generated.Constants; -import frc.robot.generated.Constants.IntakeConstants; @@ -21,6 +19,8 @@ public class HoodedShooter extends SubsystemBase{ private TalonFX hoodMotor; private final MotionMagicVelocityVoltage hoodControlV = new MotionMagicVelocityVoltage(0); private final MotionMagicVoltage hoodControl = new MotionMagicVoltage(0); + private double hoodSetpointDegrees = 0.0; + private double lastCommandedDegrees = 0.0; public HoodedShooter(){ hoodMotor = new TalonFX(15); @@ -35,8 +35,8 @@ public HoodedShooter(){ slot0.kP = Constants.HoodedShooterConstants.hoodP; slot0.kI = Constants.HoodedShooterConstants.hoodI; slot0.kD = Constants.HoodedShooterConstants.hoodD; - hoodMotorConfig.MotionMagic.MotionMagicCruiseVelocity = IntakeConstants.cruiseVelocityRps; - hoodMotorConfig.MotionMagic.MotionMagicAcceleration = IntakeConstants.accelRps2; + hoodMotorConfig.MotionMagic.MotionMagicCruiseVelocity = Constants.HoodedShooterConstants.cruiseVelocityRps; + hoodMotorConfig.MotionMagic.MotionMagicAcceleration = Constants.HoodedShooterConstants.accelRps2; hoodMotorConfig.CurrentLimits.SupplyCurrentLimit = 10; hoodMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; @@ -47,49 +47,51 @@ public HoodedShooter(){ //TODO: Make the degrees negative if hood moves in the wrong direction public void moveHoodToSetpoint(double angleInDegrees){ - //Converts to motor rotations - hoodMotor.setControl(hoodControl.withPosition(angleInDegrees/360)); + // Convert hood degrees → motor rotations via gear ratio + double rotations = (angleInDegrees / 360.0) * Constants.HoodedShooterConstants.motorRotationsPerHoodRotation; + lastCommandedDegrees = angleInDegrees; + hoodMotor.setControl(hoodControl.withPosition(rotations)); } public void zeroHood(){ hoodMotor.setPosition(0); } - //Use this method to set HoodedShooter angle based on these - public double calculateDesiredAngle(double distanceToHub, double speed){ - //TODO: Change y (height between shooter and hub) - return Math.toDegrees(getLowAngle(distanceToHub, 8, speed)); - } - - /* - * Calculates the angle (helper method, dont use anywhere else) - * - * distance to hub & height (x&y): meters - * speed (v): meters/second - * g (gravity constant): metrs per second squared - * - * returns in radian - */ - private static double getLowAngle(double x, double y, double v) { - double inside = Math.pow(v,4) - 9.81*(9.81*Math.pow(x,2) + 2*y*Math.pow(v,2)); - if (inside < 0) return 0; - double sqrt = Math.sqrt(inside); - return Math.atan((Math.pow(v,2) - sqrt) / (9.81*x)); - } - // public static double getHighAngle(double x, double y, double v) { - // double inside = Math.pow(v,4) - 9.81*(9.81*Math.pow(x,2) + 2*y*Math.pow(v,2)); - // if (inside < 0) return 0; - // double sqrt = Math.sqrt(inside); - // return Math.atan((Math.pow(v,2) + sqrt) / (9.81*x)); - // } public void moveHood(double speed){ hoodMotor.setControl(hoodControlV.withVelocity((speed))); } + /** D-pad: steps the hood setpoint by ±5° and holds position via Motion Magic. */ + public void stepHood(double deltaDegrees) { + hoodSetpointDegrees = MathUtil.clamp( + hoodSetpointDegrees + deltaDegrees, + Constants.HoodedShooterConstants.hoodMinDegrees, + Constants.HoodedShooterConstants.hoodMaxDegrees + ); + moveHoodToSetpoint(hoodSetpointDegrees); + } + + /** SOTM: commands hood to LUT-looked-up base angle + driver trim offset. */ + public void moveHoodToAngleWithOffset(double baseAngleDegrees) { + double target = MathUtil.clamp( + baseAngleDegrees + hoodSetpointDegrees, + Constants.HoodedShooterConstants.hoodMinDegrees, + Constants.HoodedShooterConstants.hoodMaxDegrees + ); + moveHoodToSetpoint(target); + } + + @Override public void periodic(){ - SmartDashboard.putNumber("HoodedShooter/HoodAngle", (hoodMotor.getPosition().getValueAsDouble())/360.0); - SmartDashboard.putNumber("HoodedShooter/HoodTarget", (hoodControl.getPositionMeasure().in(Degrees))); + // Convert motor rotations back to hood degrees using gear ratio + double actualHoodDegrees = hoodMotor.getPosition().getValueAsDouble() + / Constants.HoodedShooterConstants.motorRotationsPerHoodRotation * 360.0; + SmartDashboard.putNumber("HoodedShooter/HoodAngle", actualHoodDegrees); + SmartDashboard.putNumber("HoodedShooter/HoodSetpoint", hoodSetpointDegrees); + SmartDashboard.putNumber("HoodedShooter/HoodTarget", lastCommandedDegrees); + SmartDashboard.putNumber("HoodedShooter/StatorCurrent", hoodMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/MotorRotations", hoodMotor.getPosition().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index 75bb3b5..d4ba35a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -73,14 +73,15 @@ public ShooterIOKraken() { @Override public void runShooter(double rpm) { - double rps = rpm / 60; + // rpm is flywheel RPM; scale to motor RPS via gear ratio + double rps = rpm * Constants.Shooter.FLYWHEEL_GEAR_RATIO / 60.0; krakenShooterLeft.setControl(shooterRequest.withVelocity(-rps)); } @Override public void stopShooter(double rpm) { - double rps = rpm / 60; - krakenShooterLeft.setControl(shooterRequest.withVelocity(-rps/2)); + double rps = rpm * Constants.Shooter.FLYWHEEL_GEAR_RATIO / 60.0; + krakenShooterLeft.setControl(shooterRequest.withVelocity(-rps / 2.0)); } @Override @@ -101,12 +102,13 @@ public void stopFeeder() { @Override public double getFlywheelRPM() { - return krakenShooterLeft.getVelocity().getValueAsDouble() * 60; + // Motor velocity → flywheel RPM (divide out gear ratio) + return krakenShooterLeft.getVelocity().getValueAsDouble() * 60.0 / Constants.Shooter.FLYWHEEL_GEAR_RATIO; } @Override public double getFlywheelTargetRPM() { - return shooterRequest.Velocity * 60; + return shooterRequest.Velocity * 60.0 / Constants.Shooter.FLYWHEEL_GEAR_RATIO; } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 30b19e0..e0fed38 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -3,12 +3,12 @@ import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.generated.Constants; - public class ShooterSubsystem extends SubsystemBase { // private double shootStartTime = 0; // could come in useful later, especially for logging private final PIDController shooterPidController = new PIDController(Constants.Shooter.SHOOTER_KP, Constants.Shooter.SHOOTER_KI, Constants.Shooter.SHOOTER_KD); @@ -17,7 +17,8 @@ public class ShooterSubsystem extends SubsystemBase { public enum ShooterState { IDLE, // shooter inactive PRESHOOT, // shooter spinning up, waiting for hood to come into position, or waiting for robot to turn to goal - SHOOT // shooting + SHOOT, // shooting + SHOOT_INIT } private ShooterState state = ShooterState.IDLE; @@ -55,9 +56,11 @@ public Command changeState(ShooterState newState){ return Commands.parallel( Commands.runOnce(()->io.runShooter(targetRPM)), Commands.waitUntil(() -> isShooterReady()).andThen(Commands.runOnce(() -> { - this.state = ShooterState.SHOOT; - io.runShooter(targetRPM); - io.runFeeder(); + if (state == ShooterState.PRESHOOT) { + this.state = ShooterState.SHOOT; + io.runShooter(targetRPM); + io.runFeeder(); + } }))); case SHOOT: return Commands.parallel( @@ -67,6 +70,14 @@ public Command changeState(ShooterState newState){ Commands.runOnce(()->{ io.runFeeder(); })); + case SHOOT_INIT: + return Commands.parallel( + Commands.runOnce(()->{ + //io.runShooter(targetRPM); + }), + Commands.runOnce(()->{ + io.runFeeder(); + })); default: return Commands.none(); } @@ -86,6 +97,15 @@ public Command stopFeeder() { // double rpm = distance * Constants.Shooter.RPM_DISTANCE_MULTIPLIER + Constants.Shooter.RPM_DISTANCE_OFFSET; // this.targetRPM = rpm; // } + public void setTargetRPM(double rpm) { + this.targetRPM = rpm; + } + + /** Set target RPM and immediately command the motor. Use in continuous commands like SOTM. */ + public void commandRPM(double rpm) { + this.targetRPM = rpm; + io.runShooter(rpm); + } public void setTargetRPM(String location) { switch (location) { case "hub": @@ -108,7 +128,6 @@ public boolean isShooterReady() { @Override public void periodic() { - SmartDashboard.putNumber("Shooter/FlywheelRPM",io.getFlywheelRPM()); SmartDashboard.putNumber("Shooter/FlywheelTargetRPM",io.getFlywheelTargetRPM()); diff --git a/src/main/java/frc/robot/subsystems/vision/ProjectileSimulator.java b/src/main/java/frc/robot/subsystems/vision/ProjectileSimulator.java new file mode 100644 index 0000000..0e6b0e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ProjectileSimulator.java @@ -0,0 +1,244 @@ +/* + * ProjectileSimulator.java - RK4 projectile physics with drag and Magnus lift + * + * MIT License + * + * Copyright (c) 2026 FRC Team 5962 perSEVERE + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND. + */ + +package frc.robot.subsystems.vision; + + +/** + * Simulates a ball flying through the air with drag and Magnus lift. Uses RK4 integration + * in the vertical plane (x, z). For each distance, binary searches RPM until the ball arrives + * at the target height. Then generates a 91-point lookup table from 0.50m to 5.00m. + * + *

Basically, you plug in your robot's measurements from CAD, run generateLUT(), and it gives + * you a complete shooter table. No more hand-tuning RPM values from match videos. + * + *

Usage: + * 

+ *   ProjectileSimulator sim = new ProjectileSimulator(Constants.Vision.SOTM_PARAMETERS);
+ *   ShotLUT lut = sim.generateLUT();
+ *   // lut.get(distance) returns ShotParameters(rpm, angle, tof)
+ *
+ */ +public class ProjectileSimulator { + + // Your robot's physical measurements, from CAD and the game manual + public record SimParameters( + double ballMassKg, + double ballDiameterM, + double dragCoeff, + double magnusCoeff, + double airDensity, + double exitHeightM, + double wheelDiameterM, + double targetHeightM, + double slipFactor, + double minHoodAngleDeg, + double maxHoodAngleDeg, + double maxTofCeilingS, + double angleStepDeg, + double dt, + double rpmMin, + double rpmMax, + int binarySearchIters, + double maxSimTime) {} + + public record TrajectoryResult( + double zAtTarget, double tof, boolean reachedTarget, double maxHeight, double apexX) {} + + // One row: distance -> RPM that lands it, TOF, reachable flag + private record LUTEntry(double distanceM, double rpm, double tof, boolean reachable) {} + + private final SimParameters params; + + // Precomputed aero constants + private final double kDrag; + private final double kMagnus; + + public ProjectileSimulator(SimParameters params) { + this.params = params; + double area = Math.PI * (params.ballDiameterM() / 2.0) * (params.ballDiameterM() / 2.0); + this.kDrag = (params.airDensity() * params.dragCoeff() * area) / (2.0 * params.ballMassKg()); + this.kMagnus = + (params.airDensity() * params.magnusCoeff() * area) / (2.0 * params.ballMassKg()); + } + + /** RPM to ball exit speed (m/s). Accounts for slip between the wheel surface and ball. */ + public double exitVelocity(double rpm) { + return params.slipFactor() * rpm * Math.PI * params.wheelDiameterM() / 60.0; + } + + // state = [x, z, vx, vz] + // ax = -kDrag * |v| * vx + // az = -g - kDrag * |v| * vz + kMagnus * |v|^2 (Magnus acts as upward lift) + private double[] derivatives(double[] state) { + double svx = state[2]; + double svz = state[3]; + double speed = Math.hypot(svx, svz); + + double ax = -kDrag * speed * svx; + double az = -9.81 - kDrag * speed * svz + kMagnus * speed * speed; + + return new double[] {svx, svz, ax, az}; + } + + private static double[] addScaled(double[] base, double[] delta, double scale) { + return new double[] { + base[0] + delta[0] * scale, + base[1] + delta[1] * scale, + base[2] + delta[2] * scale, + base[3] + delta[3] * scale + }; + } + + /** Simulate a ball launched at the given RPM and explicit angle. */ + public TrajectoryResult simulate(double rpm, double targetDistanceM, double launchAngleDeg) { + double v0 = exitVelocity(rpm); + double launchRad = Math.toRadians(launchAngleDeg); + double vx = v0 * Math.cos(launchRad); + double vz = v0 * Math.sin(launchRad); + + double x = 0; + double z = params.exitHeightM(); + double dt = params.dt(); + double maxHeight = z; + double apexX = 0; + double t = 0; + double maxTime = params.maxSimTime(); + + while (t < maxTime) { + double[] state = {x, z, vx, vz}; + double[] k1 = derivatives(state); + double[] s2 = addScaled(state, k1, dt / 2.0); + double[] k2 = derivatives(s2); + double[] s3 = addScaled(state, k2, dt / 2.0); + double[] k3 = derivatives(s3); + double[] s4 = addScaled(state, k3, dt); + double[] k4 = derivatives(s4); + + x += dt / 6.0 * (k1[0] + 2 * k2[0] + 2 * k3[0] + k4[0]); + z += dt / 6.0 * (k1[1] + 2 * k2[1] + 2 * k3[1] + k4[1]); + vx += dt / 6.0 * (k1[2] + 2 * k2[2] + 2 * k3[2] + k4[2]); + vz += dt / 6.0 * (k1[3] + 2 * k2[3] + 2 * k3[3] + k4[3]); + t += dt; + + if (z > maxHeight) { maxHeight = z; apexX = x; } + + if (x >= targetDistanceM) { + double prevX = x - vx * dt; + double prevZ = z - vz * dt; + double frac = (targetDistanceM - prevX) / (x - prevX); + double zAtTarget = prevZ + frac * (z - prevZ); + double tofAtTarget = t - dt + frac * dt; + return new TrajectoryResult(zAtTarget, tofAtTarget, true, maxHeight, apexX); + } + + if (z < 0) { return new TrajectoryResult(0, t, false, maxHeight, apexX); } + } + + return new TrajectoryResult(0, maxTime, false, maxHeight, apexX); + } + + /** Binary search for RPM at a specific launch angle that lands at target height. */ + private LUTEntry findRPMForAngle(double distanceM, double launchAngleDeg) { + double heightTolerance = 0.02; + double lo = params.rpmMin(); + double hi = params.rpmMax(); + + TrajectoryResult maxCheck = simulate(hi, distanceM, launchAngleDeg); + if (!maxCheck.reachedTarget()) { + return new LUTEntry(distanceM, 0, 0, false); + } + + double bestRpm = hi; + double bestTof = maxCheck.tof(); + double bestError = Math.abs(maxCheck.zAtTarget() - params.targetHeightM()); + + for (int i = 0; i < params.binarySearchIters(); i++) { + double mid = (lo + hi) / 2.0; + TrajectoryResult result = simulate(mid, distanceM, launchAngleDeg); + + if (!result.reachedTarget()) { lo = mid; continue; } + + double error = result.zAtTarget() - params.targetHeightM(); + double absError = Math.abs(error); + + if (absError < bestError) { bestRpm = mid; bestTof = result.tof(); bestError = absError; } + if (absError < heightTolerance) { return new LUTEntry(distanceM, mid, result.tof(), true); } + + if (error > 0) { hi = mid; } else { lo = mid; } + } + + return new LUTEntry(distanceM, bestRpm, bestTof, bestError < 0.10); + } + + /** + * Generate unified shot LUT: sweep hood angles per distance, find lowest RPM + * within TOF ceiling. Takes ~5-6 seconds at startup. + */ + public ShotLUT generateLUT() { + ShotLUT lut = new ShotLUT(); + + double minAngle = params.minHoodAngleDeg(); + double maxAngle = params.maxHoodAngleDeg(); + double angleStep = params.angleStepDeg(); + double tofCeiling = params.maxTofCeilingS(); + double rpmTiebreaker = 10.0; + + for (int i = 0; i <= 90; i++) { + double distance = 0.50 + i * 0.05; + distance = Math.round(distance * 100.0) / 100.0; + + double bestRpm = Double.MAX_VALUE; + double bestAngle = minAngle; + double bestTof = tofCeiling; + + for (double angle = minAngle; angle <= maxAngle; angle += angleStep) { + LUTEntry entry = findRPMForAngle(distance, angle); + if (!entry.reachable()) continue; + if (entry.tof() > tofCeiling) continue; + + boolean betterRpm = entry.rpm() < bestRpm - rpmTiebreaker; + boolean tiedRpm = Math.abs(entry.rpm() - bestRpm) <= rpmTiebreaker; + boolean betterTof = entry.tof() < bestTof; + + if (betterRpm || (tiedRpm && betterTof)) { + bestRpm = entry.rpm(); + bestAngle = angle; + bestTof = entry.tof(); + } + } + + if (bestRpm < Double.MAX_VALUE) { + lut.put(distance, new ShotLUT.ShotParameters(bestRpm, bestAngle, bestTof)); + } + } + + return lut; + } + + // Package-private for testing + double getKDrag() { + return kDrag; + } + + double getKMagnus() { + return kMagnus; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/ShotCalculator.java b/src/main/java/frc/robot/subsystems/vision/ShotCalculator.java new file mode 100644 index 0000000..4f92643 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ShotCalculator.java @@ -0,0 +1,589 @@ +/* + * ShotCalculator.java - Newton-method SOTM fire control with drag compensation + * + * MIT License + * + * Copyright (c) 2026 FRC Team 5962 perSEVERE + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND. + */ + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.MathUtil; +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.geometry.Twist2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Shoot-on-the-move fire control solver. Figures out what RPM and heading your robot needs + * while you're driving around. It accounts for robot velocity, where the launcher is on the + * robot, processing latency, and drag on the ball during flight. + * + *

The core idea: if you're moving, you can't just aim at the target because the ball inherits + * your velocity. So we use Newton's method to find the self-consistent time-of-flight where + * the projected aim point and the LUT-predicted TOF agree. Usually converges in 2-3 iterations. + * + *

Usage: + * 

+ *   // configure for your robot (measure from CAD)
+ *   ShotCalculator.Config config = new ShotCalculator.Config();
+ *   config.launcherOffsetX = 0.23;  // meters forward of robot center
+ *   config.launcherOffsetY = 0.0;   // meters left of center
+ *
+ *   ShotCalculator calc = new ShotCalculator(config);
+ *
+ *   // load your shooter LUT (from ProjectileSimulator or hand-tuned)
+ *   for (var entry : lut.entries()) {
+ *       if (entry.reachable()) {
+ *           calc.loadLUTEntry(entry.distanceM(), entry.rpm(), entry.tof());
+ *       }
+ *   }
+ *
+ *   // call once per robot cycle
+ *   ShotCalculator.ShotInputs inputs = new ShotCalculator.ShotInputs(
+ *       swerve.getPose(), swerve.getFieldVelocity(), swerve.getRobotVelocity(),
+ *       hubCenter, hubForwardVector, visionConfidence
+ *   );
+ *   ShotCalculator.LaunchParameters result = calc.calculate(inputs);
+ *   if (result.isValid() && result.confidence() > 50) {
+ *       shooter.setRPM(result.rpm());
+ *       drivebase.setHeading(result.driveAngle());
+ *   }
+ *
+ */ +public class ShotCalculator { + + /** The result of calculate(). RPM to spin up, time of flight, heading to aim at, and a 0-100 confidence score. */ + public record LaunchParameters( + double rpm, + double timeOfFlightSec, + Rotation2d driveAngle, + double driveAngularVelocityRadPerSec, + boolean isValid, + double confidence, + double solvedDistanceM, + double hoodAngleDeg, + int iterationsUsed, + boolean warmStartUsed) { + + public static final LaunchParameters INVALID = + new LaunchParameters(0, 0, new Rotation2d(), 0, false, 0, 0, 0, 0, false); + } + + /** + * All the state the solver needs from your robot each cycle. + * pitchDeg and rollDeg are absolute tilt angles in degrees. If your gyro doesn't report + * these, just pass 0.0 for both and set config.maxTiltDeg to something huge. + */ + public record ShotInputs( + Pose2d robotPose, + ChassisSpeeds fieldVelocity, + ChassisSpeeds robotVelocity, + Translation2d hubCenter, + Translation2d hubForward, + double visionConfidence, + double pitchDeg, + double rollDeg) { + + /** Convenience constructor for callers that don't have pitch/roll data. */ + public ShotInputs( + Pose2d robotPose, + ChassisSpeeds fieldVelocity, + ChassisSpeeds robotVelocity, + Translation2d hubCenter, + Translation2d hubForward, + double visionConfidence) { + this(robotPose, fieldVelocity, robotVelocity, hubCenter, hubForward, visionConfidence, 0.0, 0.0); + } + } + + /** Tuning parameters. Set these to match your robot, or wire them to SmartDashboard/TunableNumber. */ + public static class Config { + // Launcher geometry (measure from CAD) + public double launcherOffsetX = 0.20; // meters forward of robot center + public double launcherOffsetY = 0.0; // meters left of robot center + + // How close/far you can score from (meters) + public double minScoringDistance = 0.5; + public double maxScoringDistance = 5.0; + + // Newton solver tuning + public int maxIterations = 25; + public double convergenceTolerance = 0.001; // seconds + public double tofMin = 0.05; + public double tofMax = 5.0; + + // Below this speed (m/s), don't bother with SOTM, just aim straight + public double minSOTMSpeed = 0.1; + + // Above this speed (m/s), don't shoot, we're outside calibration range + public double maxSOTMSpeed = 3.0; + + // Latency compensation (ms) + public double phaseDelayMs = 30.0; // vision pipeline lag + public double mechLatencyMs = 20.0; // how long the mechanism takes to respond + + // The ball's inherited robot velocity decays in flight because of drag. + // Real displacement = (1 - e^(-c*tof)) / c instead of just v*tof. + // Set to 0 to disable drag compensation. + public double sotmDragCoeff = 0.47; + + // Confidence scoring weights (5-component weighted geometric mean) + public double wConvergence = 1.0; + public double wVelocityStability = 0.8; + public double wVisionConfidence = 1.2; + public double wHeadingAccuracy = 1.5; + public double wDistanceInRange = 0.5; + public double headingMaxErrorRad = Math.toRadians(15); + + // Heading tolerance tightens as robot speed increases. + // scaledMaxError = base / (1 + speedScalar * speed). Set to 0 to disable. + public double headingSpeedScalar = 1.0; + + // Heading tolerance scales with distance from hub. + // Farther = tighter because the same angle error produces a larger miss at long range. + // scaledMaxError *= referenceDistance / distance, clamped [0.5, 2.0]. + public double headingReferenceDistance = 2.5; // meters + + // Suppress firing when pitch or roll exceeds this threshold. + // Bumps and ramps tilt the robot, which throws off aim. Set to 90 to disable. + public double maxTiltDeg = 5.0; + // Angle offset between robot front and shooter exit direction. + // 0 = forward-facing, PI = rear-facing. + public double shooterAngleOffsetRad = 0.0; + } + + private final Config config; + + private final ShotLUT shotLUT; + private final InterpolatingDoubleTreeMap correctionRpmMap = new InterpolatingDoubleTreeMap(); + private final InterpolatingDoubleTreeMap correctionTofMap = new InterpolatingDoubleTreeMap(); + + // Copilot RPM trim (flat offset applied during match) + private double rpmOffset = 0; + + // Solver state (reused across cycles to avoid allocation) + private double previousTOF = -1; + private double previousSpeed = 0; + + // Previous-cycle velocities for acceleration estimation + private double prevRobotVx = 0; + private double prevRobotVy = 0; + private double prevRobotOmega = 0; + + public ShotCalculator(Config config, ShotLUT shotLUT) { + this.config = config; + this.shotLUT = shotLUT; + } + + // LUT lookup: base value + any corrections + copilot RPM offset + double effectiveRPM(double distance) { + double base = shotLUT.get(distance).rpm(); + Double correction = correctionRpmMap.get(distance); + return base + (correction != null ? correction : 0.0) + rpmOffset; + } + + double effectiveTOF(double distance) { + double base = shotLUT.get(distance).tof(); + Double correction = correctionTofMap.get(distance); + return base + (correction != null ? correction : 0.0); + } + + double effectiveAngle(double distance) { + return shotLUT.get(distance).angle(); + } + + // Drag-adjusted effective TOF: actual displacement < v*tof because drag. + // Returns (1 - e^(-c*tof)) / c, or just tof if no drag. + private double dragCompensatedTOF(double tof) { + double c = config.sotmDragCoeff; + if (c < 1e-6) return tof; // no drag correction + return (1.0 - Math.exp(-c * tof)) / c; + } + + /** Central finite difference derivative of the TOF lookup table. */ + private static final double DERIV_H = 0.01; // 1cm step + + double tofMapDerivative(double d) { + double tHigh = effectiveTOF(d + DERIV_H); + double tLow = effectiveTOF(d - DERIV_H); + return (tHigh - tLow) / (2.0 * DERIV_H); + } + + /** + * Solve for the firing solution. Call once per cycle in robotPeriodic(). Returns INVALID if + * you're out of range, behind the hub, going too fast, or the inputs are bad. + */ + public LaunchParameters calculate(ShotInputs inputs) { + if (inputs == null || inputs.robotPose() == null + || inputs.fieldVelocity() == null || inputs.robotVelocity() == null) { + return LaunchParameters.INVALID; + } + + Pose2d rawPose = inputs.robotPose(); + ChassisSpeeds fieldVel = inputs.fieldVelocity(); + ChassisSpeeds robotVel = inputs.robotVelocity(); + + double poseX = rawPose.getX(); + double poseY = rawPose.getY(); + if (Double.isNaN(poseX) || Double.isNaN(poseY) + || Double.isInfinite(poseX) || Double.isInfinite(poseY)) { + return LaunchParameters.INVALID; + } + + // Second-order pose prediction. Instead of just v*dt, we use v*dt + 0.5*a*dt^2 + // where acceleration is estimated from the velocity delta between this cycle and last. + // This tracks better through turns and speed changes because it catches the curvature. + double dt = config.phaseDelayMs / 1000.0; + double ax = (robotVel.vxMetersPerSecond - prevRobotVx) / 0.02; + double ay = (robotVel.vyMetersPerSecond - prevRobotVy) / 0.02; + double aOmega = (robotVel.omegaRadiansPerSecond - prevRobotOmega) / 0.02; + Pose2d compensatedPose = + rawPose.exp( + new Twist2d( + robotVel.vxMetersPerSecond * dt + 0.5 * ax * dt * dt, + robotVel.vyMetersPerSecond * dt + 0.5 * ay * dt * dt, + robotVel.omegaRadiansPerSecond * dt + 0.5 * aOmega * dt * dt)); + prevRobotVx = robotVel.vxMetersPerSecond; + prevRobotVy = robotVel.vyMetersPerSecond; + prevRobotOmega = robotVel.omegaRadiansPerSecond; + + double robotX = compensatedPose.getX(); + double robotY = compensatedPose.getY(); + double heading = compensatedPose.getRotation().getRadians(); + + Translation2d hubCenter = inputs.hubCenter(); + double hubX = hubCenter.getX(); + double hubY = hubCenter.getY(); + + // Behind-hub detection: dot product with hub forward vector + Translation2d hubForward = inputs.hubForward(); + double dot = + (hubX - robotX) * hubForward.getX() + (hubY - robotY) * hubForward.getY(); + if (dot < 0) { + return LaunchParameters.INVALID; + } + + // Tilt gate. Bumps and ramps knock the launcher off-axis, so + // suppress firing when the chassis is tilted beyond the threshold. + if (Math.abs(inputs.pitchDeg()) > config.maxTiltDeg + || Math.abs(inputs.rollDeg()) > config.maxTiltDeg) { + return LaunchParameters.INVALID; + } + + // Transform robot center to launcher position + double cosH = Math.cos(heading); + double sinH = Math.sin(heading); + double launcherX = + robotX + config.launcherOffsetX * cosH - config.launcherOffsetY * sinH; + double launcherY = + robotY + config.launcherOffsetX * sinH + config.launcherOffsetY * cosH; + + // Launcher velocity includes rotational component: v_launcher = v_robot + omega x r + double launcherFieldOffX = config.launcherOffsetX * cosH - config.launcherOffsetY * sinH; + double launcherFieldOffY = config.launcherOffsetX * sinH + config.launcherOffsetY * cosH; + double omega = fieldVel.omegaRadiansPerSecond; + double vx = fieldVel.vxMetersPerSecond + (-launcherFieldOffY) * omega; + double vy = fieldVel.vyMetersPerSecond + launcherFieldOffX * omega; + + // Displacement from launcher to hub + double rx = hubX - launcherX; + double ry = hubY - launcherY; + double distance = Math.hypot(rx, ry); + + if (distance < config.minScoringDistance || distance > config.maxScoringDistance) { + return LaunchParameters.INVALID; + } + + double robotSpeed = Math.hypot(vx, vy); + + // Speed cap: shots above this speed are out of calibration range + if (robotSpeed > config.maxSOTMSpeed) { + return LaunchParameters.INVALID; + } + + boolean velocityFiltered = robotSpeed < config.minSOTMSpeed; + + double solvedTOF; + double projDist; + int iterationsUsed; + boolean warmStartUsed; + + if (velocityFiltered) { + // Static shot: no velocity compensation needed + solvedTOF = effectiveTOF(distance); + projDist = distance; + iterationsUsed = 0; + warmStartUsed = false; + } else { + // Newton-method SOTM solver + int maxIter = config.maxIterations; + double convTol = config.convergenceTolerance; + + // Warm start from previous cycle's solution when available + double tof; + if (previousTOF > 0) { + tof = previousTOF; + warmStartUsed = true; + } else { + tof = effectiveTOF(distance); + warmStartUsed = false; + } + + projDist = distance; + iterationsUsed = 0; + + for (int i = 0; i < maxIter; i++) { + double prevTOF = tof; + + // Compute drag exponent once per iteration for both drift and derivative + double c = config.sotmDragCoeff; + double dragExp = c < 1e-6 ? 1.0 : Math.exp(-c * tof); + double driftTOF = c < 1e-6 ? tof : (1.0 - dragExp) / c; + + // Projected displacement at time t, with drag-compensated velocity offset + double prx = rx - vx * driftTOF; + double pry = ry - vy * driftTOF; + projDist = Math.hypot(prx, pry); + + // Degenerate guard: ball is essentially on top of the hub + if (projDist < 0.01) { + tof = effectiveTOF(distance); + iterationsUsed = maxIter + 1; // flag as diverged + break; + } + + double lookupTOF = effectiveTOF(projDist); + + // Derivative for Newton step (chain rule: d/dt of dragCompensatedTOF = e^(-ct)) + double dPrime = -dragExp * (prx * vx + pry * vy) / projDist; + double gPrime = tofMapDerivative(projDist); + double f = lookupTOF - tof; + double fPrime = gPrime * dPrime - 1.0; + + // Newton step with near-zero denominator guard + if (Math.abs(fPrime) > 0.01) { + tof = tof - f / fPrime; + } else { + tof = lookupTOF; // fixed-point fallback + } + + // Per-iteration clamp prevents runaway + tof = MathUtil.clamp(tof, config.tofMin, config.tofMax); + + iterationsUsed = i + 1; + + // Convergence check + if (Math.abs(tof - prevTOF) < convTol) { + break; + } + } + + // Divergence guard + if (tof > config.tofMax || tof < 0.0 || Double.isNaN(tof)) { + tof = effectiveTOF(distance); + iterationsUsed = maxIter + 1; + } + + solvedTOF = tof; + } + + // Save for next cycle's warm start + previousTOF = solvedTOF; + + double effectiveTOF = solvedTOF + config.mechLatencyMs / 1000.0; + + // RPM from LUT at solved distance + double effectiveRPMValue = effectiveRPM(projDist); + + // Drive angle: aim at velocity-compensated target position + double compTargetX; + double compTargetY; + if (velocityFiltered) { + compTargetX = hubX; + compTargetY = hubY; + } else { + double headingDriftTOF = dragCompensatedTOF(solvedTOF); + compTargetX = hubX - vx * headingDriftTOF; + compTargetY = hubY - vy * headingDriftTOF; + } + double aimX = compTargetX - robotX; + double aimY = compTargetY - robotY; + Rotation2d driveAngle = new Rotation2d(aimX, aimY); + + // Heading error for confidence calculation + double headingErrorRad = MathUtil.angleModulus(driveAngle.getRadians() - heading - config.shooterAngleOffsetRad); + + // Angular velocity feedforward: rate of change of aim angle. + // Use the velocity-compensated aim vector (not raw hub displacement) so + // the feedforward matches the actual target the robot is tracking. + double driveAngularVelocity = 0; + if (!velocityFiltered && projDist > 0.1) { + double compRx = compTargetX - robotX; + double compRy = compTargetY - robotY; + double compDist = Math.hypot(compRx, compRy); + if (compDist > 0.1) { + double tangentialVel = (compRy * vx - compRx * vy) / compDist; + driveAngularVelocity = tangentialVel / compDist; + } + } + + // Solver convergence quality + double solverQuality; + if (velocityFiltered) { + solverQuality = 1.0; + } else { + int maxIter = config.maxIterations; + if (iterationsUsed > maxIter) { + solverQuality = 0.0; + } else if (iterationsUsed <= 3) { + solverQuality = 1.0; + } else { + solverQuality = + MathUtil.interpolate(1.0, 0.1, (double) (iterationsUsed - 3) / (maxIter - 3)); + } + } + + double confidence = computeConfidence( + solverQuality, robotSpeed, headingErrorRad, distance, inputs.visionConfidence()); + + previousSpeed = robotSpeed; + + double effectiveAngleValue = effectiveAngle(projDist); + + return new LaunchParameters( + effectiveRPMValue, + effectiveTOF, + driveAngle, + driveAngularVelocity, + true, + confidence, + projDist, + effectiveAngleValue, + iterationsUsed, + warmStartUsed); + } + + /** + * Confidence from 0 to 100. Weighted geometric mean of 5 factors: solver convergence, + * velocity stability, vision confidence, heading accuracy, and distance from range edges. + * If any single factor drops to zero (like vision dies), the whole score tanks to zero. + * That's intentional because you really shouldn't be shooting if any one factor is gone. + */ + private double computeConfidence( + double solverQuality, double currentSpeed, double headingErrorRad, + double distance, double visionConfidence) { + + // 1. Solver quality (passed in, already 0-1) + double convergenceQuality = solverQuality; + + // 2. Velocity stability: penalize rapid speed changes + double speedDelta = Math.abs(currentSpeed - previousSpeed); + double velocityStability = MathUtil.clamp(1.0 - speedDelta / 0.5, 0, 1); + + // 3. Vision confidence (0-1, from caller) + double visionConf = MathUtil.clamp(visionConfidence, 0, 1); + + // 4. Heading accuracy with speed scaling and distance scaling. + // Faster robot = tighter tolerance (because velocity errors compound). + // Closer to hub = tighter tolerance (because small angles mean big misses). + double distanceScale = MathUtil.clamp( + config.headingReferenceDistance / distance, 0.5, 2.0); + double speedScale = 1.0 / (1.0 + config.headingSpeedScalar * currentSpeed); + double scaledMaxError = config.headingMaxErrorRad * distanceScale * speedScale; + double headingErr = Math.abs(headingErrorRad); + double headingAccuracy = MathUtil.clamp(1.0 - headingErr / scaledMaxError, 0, 1); + + // 5. Distance in range: penalty for being near min/max scoring boundaries + double rangeSpan = config.maxScoringDistance - config.minScoringDistance; + double rangeFraction = (distance - config.minScoringDistance) / rangeSpan; + double distInRange = 1.0 - 2.0 * Math.abs(rangeFraction - 0.5); + distInRange = MathUtil.clamp(distInRange, 0, 1); + + // Weighted geometric mean (one zero kills it) + double[] c = {convergenceQuality, velocityStability, visionConf, headingAccuracy, distInRange}; + double[] w = { + config.wConvergence, + config.wVelocityStability, + config.wVisionConfidence, + config.wHeadingAccuracy, + config.wDistanceInRange + }; + + double sumW = 0; + double logSum = 0; + for (int i = 0; i < 5; i++) { + if (c[i] <= 0) return 0; + logSum += w[i] * Math.log(c[i]); + sumW += w[i]; + } + + if (sumW <= 0) return 0; + double composite = Math.exp(logSum / sumW) * 100.0; + return MathUtil.clamp(composite, 0, 100); + } + + /** Layer a per-distance RPM adjustment on top of the base LUT. Good for field tuning at comp. */ + public void addRpmCorrection(double distance, double deltaRpm) { + correctionRpmMap.put(distance, deltaRpm); + } + + /** Layer a per-distance TOF adjustment on top of the base LUT. */ + public void addTofCorrection(double distance, double deltaTof) { + correctionTofMap.put(distance, deltaTof); + } + + /** Clear all corrections, back to the raw LUT. */ + public void clearCorrections() { + correctionRpmMap.clear(); + correctionTofMap.clear(); + } + + /** Bump the RPM offset by delta. Clamped to +/- 200. Bind this to copilot D-pad. */ + public void adjustOffset(double delta) { + rpmOffset = MathUtil.clamp(rpmOffset + delta, -200, 200); + } + + /** Reset the RPM offset to zero. Call this on mode transitions so trim doesn't carry over. */ + public void resetOffset() { + rpmOffset = 0; + } + + public double getOffset() { + return rpmOffset; + } + + /** Raw time-of-flight from the LUT at this distance (no velocity compensation). */ + public double getTimeOfFlight(double distanceM) { + return effectiveTOF(distanceM); + } + + /** Base RPM at this distance, before any corrections or offset. */ + public double getBaseRPM(double distance) { + return shotLUT.get(distance).rpm(); + } + + /** Reset the warm start state. Call this after a pose reset so the solver doesn't use stale data. */ + public void resetWarmStart() { + previousTOF = -1; + previousSpeed = 0; + prevRobotVx = 0; + prevRobotVy = 0; + prevRobotOmega = 0; + } + +} diff --git a/src/main/java/frc/robot/subsystems/vision/ShotLUT.java b/src/main/java/frc/robot/subsystems/vision/ShotLUT.java new file mode 100644 index 0000000..bde40d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ShotLUT.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; + +/** LUT for a shooter with adjustable flywheel velocity (RPM), hood angle (degrees), and ToF (seconds). */ +public class ShotLUT { + public record ShotParameters(double rpm, double angle, double tof) + implements Interpolatable { + public ShotParameters interpolate(ShotParameters endValue, double t) { + return new ShotParameters( + MathUtil.interpolate(rpm(), endValue.rpm(), t), + MathUtil.interpolate(angle(), endValue.angle(), t), + MathUtil.interpolate(tof(), endValue.tof(), t)); + } + } + + private final InterpolatingTreeMap map = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShotParameters::interpolate); + + public void put(double distance, ShotParameters params) { + map.put(distance, params); + } + + public ShotParameters get(double distance) { + return map.get(distance); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/ShotTable.java b/src/main/java/frc/robot/subsystems/vision/ShotTable.java new file mode 100644 index 0000000..4ca13c9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ShotTable.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.vision; + +/** + * Hand-tuned shooter lookup table. Add measured data points here — the system + * interpolates RPM, hood angle, and time-of-flight for any distance in between. + * + * HOW TO TUNE: + * 1. Stand a known distance from the hub (use a tape measure or odometry). + * 2. Manually adjust RPM and hood angle until shots score consistently. + * 3. Add or update the corresponding row in DATA below. + * 4. Repeat for several distances spread across your full shooting range. + * 5. Interpolation fills in everything between measured points automatically. + * + * DATA FORMAT — each row: + * { distanceMeters, flywheelRPM, hoodAngleDegrees, timeOfFlightSeconds } + * + * DISTANCE — from the launcher to the hub center, in meters. + * RPM — flywheel RPM (ShooterIOKraken applies the gear ratio internally). + * ANGLE — hood angle in degrees. Must be within [hoodMinDegrees, hoodMaxDegrees]. + * TOF — time-of-flight in seconds. Only affects shoot-on-the-move compensation. + * If you don't need SOTM, a rough estimate (distance / 10) is fine. + * Tune by watching how much ball drift you see while driving. + * + * TIPS: + * - More data points = smoother interpolation. Aim for one every 0.5–1.0 m. + * - Keep distances sorted ascending so the table is easy to read. + * - You need at least 2 rows for interpolation to work. + */ +public class ShotTable { + + // ------------------------------------------------------------------------- + // EDIT THESE ROWS with your measured values. + // ------------------------------------------------------------------------- + private static final double[][] DATA = { + // { distM, rpm, angleDeg, tof } + { 1.5, 3000, 12.5, 0.30 }, + { 2.0, 3200, 14, 0.40 }, + { 3, 3500, 18.0, 0.50 }, + { 4, 3800, 20.0, 0.60 }, + // { 3.5, 3500, 20.0, 0.68 }, + // { 4.0, 3700, 20.5, 0.76 }, + // { 4.5, 3800, 21, 0.84 }, + // { 5.0, 3900, 21.5, 0.92 }, + }; + // -------------------------------------------- ----------------------------- + + /** Build a ShotLUT from the data points above. Called once at startup. */ + public static ShotLUT buildLUT() { + ShotLUT lut = new ShotLUT(); + for (double[] row : DATA) { + double dist = row[0]; + double rpm = row[1]; + double angle = row[2]; + double tof = row[3]; + lut.put(dist, new ShotLUT.ShotParameters(rpm, angle, tof)); + } + return lut; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..1f45ced --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,145 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; +import frc.robot.generated.Constants; + +/** + * Vision subsystem — uses raw Limelight tx/ty to aim at AprilTags on the hub. + * + * tx + camera yaw → shooter alignment error (degrees off from shooter axis). + * ty + trig → distance to target → ShotLUT → RPM + hood angle. + * + * No global pose estimation, no MegaTag, no Kalman filter. + */ +public class Vision extends SubsystemBase { + + // AprilTag center height on the hub (44.25 inches). + private static final double TAG_HEIGHT_M = 44.25 * 0.0254; // 1.12395 m + + // Camera mounting — from Limelight UI (both cameras identical). + private static final double CAMERA_HEIGHT_M = 0.2439162; + private static final double CAMERA_PITCH_DEG = 20.0; + + // Camera yaw offsets from robot forward (from Limelight UI). + // Shooter faces 180° from robot forward. + private static final double LEFT_CAM_YAW_DEG = 150.0; + private static final double RIGHT_CAM_YAW_DEG = -150.0; + private static final double SHOOTER_YAW_DEG = 180.0; + + // ------------------------------------------------------------------------- + + private final ShotLUT lut; + + // Copilot RPM trim (flat offset applied during match). + private double rpmOffset = 0; + + // Cached per-cycle results, read by AutoAlign. + private boolean hasTarget = false; + private double shooterErrorDeg = 0; // positive = target is CW from shooter axis + private double distanceM = 0; + private double targetRPM = 0; + private double targetHoodAngleDeg = 0; + + public Vision() { + lut = ShotTable.buildLUT(); + } + + @Override + public void periodic() { + String bestCam = pickBestCamera(); + hasTarget = (bestCam != null); + + if (hasTarget) { + double tx = LimelightHelpers.getTX(bestCam); + double ty = LimelightHelpers.getTY(bestCam); + double camYaw = bestCam.equals(Constants.Vision.primaryLimelightName) + ? LEFT_CAM_YAW_DEG : RIGHT_CAM_YAW_DEG; + + // Shooter alignment error. + // Limelight tx: positive = target to the RIGHT of camera center. + // In CCW-positive robot frame, "right" = decreasing angle, so: + // target_angle = camYaw - tx + // We want the PID to output positive → CCW when the target is CCW from the shooter, + // and negative → CW when the target is CW from the shooter. + // error = SHOOTER_YAW - target_angle = 180 - camYaw + tx + // When error > 0: target is CW from shooter → PID outputs negative → CW rotation ✓ + // When error < 0: target is CCW from shooter → PID outputs positive → CCW rotation ✓ + shooterErrorDeg = MathUtil.inputModulus(SHOOTER_YAW_DEG - camYaw + tx, -180, 180); + + // Distance via trig: d = (tagH - camH) / tan(camPitch + ty) + double angleDeg = CAMERA_PITCH_DEG + ty; + double angleRad = Math.toRadians(angleDeg); + if (angleRad > 0.01) { + distanceM = (TAG_HEIGHT_M - CAMERA_HEIGHT_M) / Math.tan(angleRad); + } else { + distanceM = 0; + } + + // Clamp distance to LUT range for sensible outputs. + double clampedDist = MathUtil.clamp(distanceM, 1.5, 4.0); + ShotLUT.ShotParameters params = lut.get(clampedDist); + targetRPM = params.rpm() + rpmOffset; + targetHoodAngleDeg = params.angle(); + } else { + shooterErrorDeg = 0; + distanceM = 0; + targetRPM = 0; + targetHoodAngleDeg = 0; + } + + // Telemetry + SmartDashboard.putBoolean("Vision/HasTarget", hasTarget); + SmartDashboard.putNumber("Vision/ShooterErrorDeg", shooterErrorDeg); + SmartDashboard.putNumber("Vision/DistanceM", distanceM); + SmartDashboard.putNumber("Vision/TargetRPM", targetRPM); + SmartDashboard.putNumber("Vision/HoodAngleDeg", targetHoodAngleDeg); + SmartDashboard.putNumber("Vision/RPMOffset", rpmOffset); + } + + /** + * Returns the camera name that currently has a valid target. + * Prefers the camera whose target is closer to the shooter axis. + * Returns null if neither sees a tag. + */ + private String pickBestCamera() { + boolean leftHas = LimelightHelpers.getTV(Constants.Vision.primaryLimelightName); + boolean rightHas = LimelightHelpers.getTV(Constants.Vision.secondaryLimelightName); + if (leftHas && rightHas) { + double leftError = Math.abs(MathUtil.inputModulus( + SHOOTER_YAW_DEG - LEFT_CAM_YAW_DEG + + LimelightHelpers.getTX(Constants.Vision.primaryLimelightName), -180, 180)); + double rightError = Math.abs(MathUtil.inputModulus( + SHOOTER_YAW_DEG - RIGHT_CAM_YAW_DEG + + LimelightHelpers.getTX(Constants.Vision.secondaryLimelightName), -180, 180)); + return (leftError <= rightError) ? Constants.Vision.primaryLimelightName + : Constants.Vision.secondaryLimelightName; + } + if (leftHas) return Constants.Vision.primaryLimelightName; + if (rightHas) return Constants.Vision.secondaryLimelightName; + return null; + } + + // --- Public getters for AutoAlign --- + + public boolean hasTarget() { return hasTarget; } + /** Shooter-to-target error in degrees. Positive = target is CW from shooter (robot should rotate CW). */ + public double getShooterErrorDeg() { return shooterErrorDeg; } + public double getDistanceM() { return distanceM; } + public double getTargetRPM() { return targetRPM; } + public double getTargetHoodAngleDeg() { return targetHoodAngleDeg; } + + // --- Copilot offset controls --- + + /** Bump the RPM offset by delta. Clamped to +/- 200. Bind to copilot D-pad. */ + public void adjustOffset(double delta) { + rpmOffset = MathUtil.clamp(rpmOffset + delta, -200, 200); + } + + /** Reset the RPM offset to zero. */ + public void resetOffset() { + rpmOffset = 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/limelight_vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/limelight_vision/Vision.java deleted file mode 100644 index f64b5c5..0000000 --- a/src/main/java/frc/robot/subsystems/vision/limelight_vision/Vision.java +++ /dev/null @@ -1,188 +0,0 @@ -package frc.robot.subsystems.vision.limelight_vision; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.generated.Constants; -import frc.robot.subsystems.Swerve; - -public class Vision extends SubsystemBase { - private final NetworkTable limelightLeft; - private final NetworkTable limelightRight; - private final Swerve swerve; - private final Pose3d camPosePrimary; - private final Pose3d camPoseSecondary; - - // Cached for SmartDashboard logging - private double lastBearingDeg = 0; - private double lastTagRobotX = 0; - private double lastTagRobotY = 0; - - public Vision(Swerve swerve) { - this.swerve = swerve; - limelightLeft = NetworkTableInstance.getDefault().getTable(Constants.Vision.primaryLimelightName); - limelightRight = NetworkTableInstance.getDefault().getTable(Constants.Vision.secondaryLimelightName); - camPosePrimary = Constants.Vision.cameraPosePrimary != null ? Constants.Vision.cameraPosePrimary : new Pose3d(); - camPoseSecondary = Constants.Vision.cameraPoseSecondary != null ? Constants.Vision.cameraPoseSecondary : new Pose3d(); - } - - /** Returns true if at least one camera currently sees a valid hub tag for the given alliance. */ - public boolean hasHubTarget(boolean isRed) { - double tvLeft = limelightLeft.getEntry("tv").getDouble(0.0); - double tvRight = limelightRight.getEntry("tv").getDouble(0.0); - if (tvLeft == 1.0) { - int tagId = (int) limelightLeft.getEntry("tid").getDouble(-1); - if (isHubTag(tagId, isRed)) return true; - } - if (tvRight == 1.0) { - int tagId = (int) limelightRight.getEntry("tid").getDouble(-1); - if (isHubTag(tagId, isRed)) return true; - } - return false; - } - - /** Returns true if the given tag ID belongs to the correct alliance's hub. */ - private boolean isHubTag(int tagId, boolean isRed) { - int[] hubTags = isRed ? Constants.Vision.RED_HUB_TAGS : Constants.Vision.BLUE_HUB_TAGS; - for (int id : hubTags) { - if (id == tagId) return true; - } - return false; - } - - /** - * Converts a tag pose from camera space to robot space using the camera's mount pose. - * Returns null if the pose data is invalid (all-zero NT default). - */ - private Pose3d tagCamToRobotSpace(double[] arr, Pose3d camPose) { - // Reject zero/near-zero arrays — NT default before real data arrives - if (arr[0] * arr[0] + arr[1] * arr[1] + arr[2] * arr[2] < 0.01) { - return null; - } - // Limelight targetpose_cameraspace: X=right, Y=up, Z=forward - // WPILib camera frame: X=forward, Y=left, Z=up - Pose3d tagInCam = new Pose3d( - new Translation3d(arr[2], -arr[0], arr[1]), - new Rotation3d(Math.toRadians(arr[3]), Math.toRadians(arr[4]), Math.toRadians(arr[5])) - ); - return camPose.transformBy(new Transform3d(tagInCam.getTranslation(), tagInCam.getRotation())); - } - - /** - * Returns the absolute field heading the robot should face to point its launcher toward - * the score pillar. Only uses readings from the correct alliance's hub tags. - */ - public Rotation2d getHeadingToScorePillar(boolean isRed) { - double tvLeft = limelightLeft.getEntry("tv").getDouble(0.0); - double tvRight = limelightRight.getEntry("tv").getDouble(0.0); - if (tvLeft < 0.5 && tvRight < 0.5) { - return new Rotation2d(); - } - - // Use tx (horizontal angle, positive=right) + camera yaw to get bearing in robot frame. - // bearing_robot_deg = camera_yaw_deg - tx_deg (tx positive = clockwise from camera center) - double sumSin = 0, sumCos = 0; - int count = 0; - - if (tvLeft > 0.5) { - int tagId = (int) limelightLeft.getEntry("tid").getDouble(-1); - if (isHubTag(tagId, isRed)) { - double txDeg = limelightLeft.getEntry("tx").getDouble(0.0); - double camYawDeg = Math.toDegrees(camPosePrimary.getRotation().getZ()); - double bearingRad = Math.toRadians(camYawDeg - txDeg); - sumSin += Math.sin(bearingRad); - sumCos += Math.cos(bearingRad); - count++; - } - } - - if (tvRight > 0.5) { - int tagId = (int) limelightRight.getEntry("tid").getDouble(-1); - if (isHubTag(tagId, isRed)) { - double txDeg = limelightRight.getEntry("tx").getDouble(0.0); - double camYawDeg = Math.toDegrees(camPoseSecondary.getRotation().getZ()); - double bearingRad = Math.toRadians(camYawDeg - txDeg); - sumSin += Math.sin(bearingRad); - sumCos += Math.cos(bearingRad); - count++; - } - } - - if (count == 0) return new Rotation2d(); - - double bearingRad = Math.atan2(sumSin, sumCos); - lastBearingDeg = Math.toDegrees(bearingRad); - lastTagRobotX = 0; - lastTagRobotY = 0; - - // +PI because the launcher faces the back of the robot - return swerve.getHeading().plus(new Rotation2d(bearingRad + Math.PI)); - } - - /** - * Returns the 2D distance (meters) from the robot to the score pillar, or NaN if no target. - * Only uses readings from the correct alliance's hub tags. - */ - public double getDistanceToScorePillar(boolean isRed) { - double tvLeft = limelightLeft.getEntry("tv").getDouble(0.0); - double tvRight = limelightRight.getEntry("tv").getDouble(0.0); - if (tvLeft < 0.5 && tvRight < 0.5) { - return Double.NaN; - } - - Pose3d leftRobot = null; - Pose3d rightRobot = null; - - if (tvLeft > 0.5) { - int tagId = (int) limelightLeft.getEntry("tid").getDouble(-1); - if (isHubTag(tagId, isRed)) { - double[] arr = limelightLeft.getEntry("targetpose_cameraspace").getDoubleArray(new double[6]); - leftRobot = tagCamToRobotSpace(arr, camPosePrimary); - } - } - if (tvRight > 0.5) { - int tagId = (int) limelightRight.getEntry("tid").getDouble(-1); - if (isHubTag(tagId, isRed)) { - double[] arr = limelightRight.getEntry("targetpose_cameraspace").getDoubleArray(new double[6]); - rightRobot = tagCamToRobotSpace(arr, camPoseSecondary); - } - } - - double dx, dy; - if (leftRobot != null && rightRobot != null) { - dx = (leftRobot.getX() + rightRobot.getX()) / 2.0; - dy = (leftRobot.getY() + rightRobot.getY()) / 2.0; - } else if (leftRobot != null) { - dx = leftRobot.getX(); - dy = leftRobot.getY(); - } else if (rightRobot != null) { - dx = rightRobot.getX(); - dy = rightRobot.getY(); - } else { - return Double.NaN; - } - - return Math.hypot(dx, dy); - } - - public void periodic() { - SmartDashboard.putNumber("Vision/TagRobotX", lastTagRobotX); - SmartDashboard.putNumber("Vision/TagRobotY", lastTagRobotY); - SmartDashboard.putNumber("Vision/BearingDeg", lastBearingDeg); - SmartDashboard.putNumber("Vision/RobotHeadingDeg", swerve.getHeading().getDegrees()); - SmartDashboard.putData("Vision/AutoAlignPID", Constants.Vision.rotationPID); - boolean isRed = DriverStation.getAlliance().isPresent() && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - SmartDashboard.putNumber("Vision/Distance", getDistanceToScorePillar(isRed)); - SmartDashboard.putNumber("Vision/TargetHeading", getHeadingToScorePillar(isRed).getDegrees()); - - - } -}