2026 Rebuilt — FRC Robot Code

中文版

Team 6940's offseason robot code for the 2026 FIRST Robotics Competition game REBUILT.

Overview

This is a Java 17 WPILib project using the Command-Based framework with AdvantageKit logging and replay support. The robot features an SDS Mk5n swerve drivetrain, an over-the-bumper intake with motion-magic rack positioning, a full-width shooter, and a fused Limelight + PhotonVision vision pipeline (experimental).

Project Structure

src/main/java/frc/robot/
├── Main.java                          # Entry point
├── Robot.java                         # LoggedRobot — mode dispatch (REAL/SIM/REPLAY)
├── RobotContainer.java                # Subsystem instantiation, button bindings, auto chooser
├── Constants.java                     # All tuning constants, field geometry, motor IDs
├── Telemetry.java                     # Swerve module telemetry to NetworkTables + SignalLogger
├── BuildConstants.java                # Auto-generated build metadata (do not edit)
├── generated/
│   └── TunerConstants.java            # Auto-generated by Phoenix 6 Tuner (do not edit)
├── commands/                          # All commands
│   ├── Autos/                         # Autonomous routines (PathPlanner-based)
│   ├── DriveDefaultCommand.java       # Teleop swerve drive
│   ├── DriveHybridTrenchCommand.java  # Trench shared-control drive
│   ├── IntakeHybridCommand.java       # LT intake with hold-to-hybrid
│   ├── HybridScoreCommand.java        # Hub scoring: AIM → READY → SHOOT
│   ├── HybridPassCommand.java         # Pass lane shooting
│   ├── ManualShootCommand.java        # SmartDashboard-tuned shooting
│   ├── HeatupCommand.java             # Shooter pre-spin
│   └── leds/LEDDefaultCommand.java    # LED state machine
├── subsystems/
│   ├── SuperStructure.java            # Central mode coordinator (DriveMode, ControlMode, ShootPhase, IntakeMode)
│   ├── Chassis/
│   │   ├── CommandSwerveDrivetrain.java  # SDS Mk5n swerve + PathPlanner + Trench assist
│   │   ├── SwerveDriveIO.java            # AdvantageKit IO interface
│   │   ├── TrenchLane.java               # Trench centerline path enum
│   │   └── HybridTrenchReference.java    # Trench guidance math
│   ├── Intake/                        # Over-the-bumper intake: rack (Motion Magic) + dual rollers
│   ├── Shooter/                       # Full-width rear dumper shooter
│   ├── Hood/                          # Single motor, Motion Magic position control
│   ├── Indexer/                       # Feeder + indexer rollers, jam detection
│   ├── Vision/                        # Limelight MegaTag2 + PhotonVision fusion (experimental)
│   ├── Halo/LEDController.java        # Addressable LED strip with layered animation
│   ├── Power/PowerMonitor.java        # Centralized current/power logging
│   └── GamePeriodReminder.java        # Match-time shift reminder
├── simulation/
│   └── FieldSimulation.java           # Maple-sim field physics (intake, hopper, dumper shooter)
├── util/                              # Math helpers, interpolation, PathPlanner AD*
└── Library/                           # Community code (team 95, 1323, 1678, 1706, 2910, 3476, 503, 6940)

Architecture

IO Abstraction Pattern

Every hardware subsystem follows the same pattern for clean simulation/replay support:

Layer	Purpose	Example
*IO interface	Defines all hardware reads/writes + @AutoLog inputs	IntakeIO
*IOPhoenix6	Real CTRE Phoenix 6 hardware implementation	IntakeIOPhoenix6
*IOSim	Maple-sim physics simulation	IntakeIOSim

The subsystem selects the implementation at construction time based on Constants.currentMode:

if (Constants.currentMode == Mode.REAL)      io = new IntakeIOPhoenix6();
else if (Constants.currentMode == Mode.SIM)  io = new IntakeIOSim();
else                                         io = new IntakeIO() {};  // replay: no-op

SuperStructure — Central Mode Coordinator

SuperStructure is the robot's mode hub. Commands read and write four enums:

Enum	Values	Purpose
DriveMode	AUTO_AIM, HYBRID_TRENCH, HYBRID_INTAKE_DRIVE, MANUAL	Drive control priority
ControlMode	SCORE, PASS, MANUAL	Shoot target selection
ShootPhase	OFF, HEATUP, AIM, READY, SHOOT	Shooting sequence state
IntakeMode	INTAKE, HYBRID, RETRACTED, OFF, REVERSE, MID	Intake rack position

Drive mode priority (highest wins): AutoAim > HybridTrench > HybridIntake > Manual.

Shooting Pipeline

The shooter uses a motion-compensated ballistics solver (ProjectileCalculator):

1. HEATUP — Pre-spin shooter flywheel to target velocity
2. AIM — Auto-aim chassis heading toward hub center; set hood angle from distance lookup table
3. READY — All subsystems within tolerance (heading, hood, shooter RPM)
4. SHOOT — Lock chassis, feed ball via indexer, then retract intake

Two shot profiles: SCORE (hub center, full velocity) and PASS (pass lane, lower velocity with flight-time compensation). Operator can trim RPS with controller buttons (B/A/X/Y = -1/-2/+1/+2).

Switchable Motion Solver

ProjectileCalculator provides two ballistics modes, switchable per shoot command. The motion solver approach is adapted from Team 6328 (Mechanical Advantage).

Mode	Method	Behavior
Motion Solver	planScore() / planPass()	Iterative 20-iteration lookahead: computes a virtual target offset by fieldVelocity * flightTime so the ball meets the real target despite chassis movement. Logged as usesMotionSolver = true.
Fixed-Position	planFixScore() / planManual()	Static distance-to-target lookup from InterpolatingDoubleTreeMap tables. No velocity compensation. Logged as usesMotionSolver = false.

Currently HybridScoreCommand uses fixed-position mode (motion-solver path is commented out but available), while HybridPassCommand always uses the motion solver — pass shots require velocity compensation because the robot is typically driving at speed. The two modes share the same distance lookup tables but with profile-specific entries (DistanceToShooterRps vs PassDistanceToShooterRps).

Hybrid Trench — Driving Assist

The Trench drive (driveHybridTrench() in CommandSwerveDrivetrain) is a shared-control system that blends driver joystick input with PathPlanner path guidance:

1. Path selection — Two trench centerline splines (ATLtoNTL, ATRtoNTR) are loaded at startup. The nearest lane is auto-selected based on robot position. Paths auto-flip for red/blue alliance and for the robot's half of the field.

2. Lookahead guidance — Finds the nearest point on the selected path, then samples a lookahead point 0.6 m ahead (configurable via TrenchLookAheadMeters). A guidance vector points from the robot toward the lookahead.

3. Driver blending — The driver's joystick velocity is blended with the guidance vector. Cross-track distance controls adhesion: when the robot is on the path, guidance dominates; when far off-path, driver intent dominates. Adhesion follows a sqrt curve (TrenchBlendExponent = 0.5) for smooth falloff.

4. Direction inference — The system detects traversal direction from driver intent (dot product of joystick field velocity with path tangent). This avoids requiring a separate button to switch direction — just push the stick the way you want to go.

5. Heading hold — When IntakeMode is HYBRID, the chassis auto-snaps to the nearest trench square edge (with 20-degree hysteresis to prevent oscillation). Right stick overrides manual heading. A cosine-based linear speed reduction (kLiner = cos(headingError)) naturally slows the robot during heading corrections.

6. Activation — Driver holds A to enter trench mode. RT while in trench mode activates IntakeMode.HYBRID for simultaneous pickup. Speed is capped at 60% of max for safety.

ChassisMode — Current Limit Profiles

Three profiles managed by SuperStructure:

Mode	Drive Supply	Steer Supply	Use Case
NORMAL	60A	50A	Default driving
SHOOTING	20A	25A	Frees battery for shooter
ATTACKMODE	Unlimited	Unlimited	Full power (10s timeout)

Vision Fusion (Experimental)

Fuses two vision sources with inverse-variance weighting:

• Limelight — MegaTag2 pose estimation with hardware-reported standard deviations
• PhotonVision — AprilTag detection with target-area-based sigma interpolation

Rejection gates: max distance, stale data, high angular velocity, minimum target area. Hub tags are preferred over trench tags for scoring accuracy.

Simulation

Desktop simulation uses maple-sim for physics:

• Swerve drive collision body with configurable mass (74 kg), MOI, and wheel COF
• Over-the-bumper intake simulation (front-mounted, configurable width/extension)
• Full-width rear dumper shooter with parallel fuel lanes
• Field spawns fuel objects for intake testing

To switch between simulation and log replay, change Constants.simMode:

public static final Mode simMode = Mode.SIM;     // Physics simulation
public static final Mode simMode = Mode.REPLAY;  // Replay from .wpilog file

Autonomous

9 autonomous routines built with PathPlannerLib, selectable via SmartDashboard:

Routine	Description
RightDoubleSwipe	Right start, two intake-score cycles
RightDoubleSwipeOverMid	Right start, crosses mid-field for second intake
RightDoubleSwipeTrench	Right start, uses trench lane for second intake
LeftDoubleSwipe	Left start mirror of right
LeftDoubleSwipeOverMid	Left start, crosses mid-field
LeftDoubleSwipeTrench	Left start, uses trench lane
LeftDepot	Left start, depot pickup
LeftDepotSingleSwipe	Left start, depot + single swipe

All routines support auto-delay via SmartDashboard/Auto Delay and alliance-color flipping.

Vendor Libraries

Library	Version	Purpose
AdvantageKit	26.0.2	Logging, replay, @AutoLog annotation processor
Phoenix 6	26.3.0	CTRE TalonFX motor controllers, Pigeon IMU
PathPlannerLib	2026.1.2	Autonomous path following + pathfinding
PhotonLib	—	PhotonVision AprilTag detection
ChoreoLib	2026	Choreography support
maple-sim	—	Physics simulation framework
Studica	—	NavX IMU support
URCL	—	Universal robot control library
libgrapplefrc2026	—	Grapple FRC utilities
WPILibNewCommands	—	Command-based framework

Configuration

Key constants in Constants.java:

• simMode — Mode.SIM (physics sim) or Mode.REPLAY (log replay)
• enableNtTelemetry — Set false during competition to avoid NT4 publish latency
• MotorIDs — All CAN IDs for chassis, intake, shooter, hood, indexer
• FieldConstants — AprilTag layout, hub geometry, trench/bump dimensions
• ProjectileConstants — Distance-to-RPS and distance-to-hood lookup tables

AprilTag field layout loaded from src/main/deploy/pathplanner/field2026/2026-official-andymark.json.

Telemetry

• AdvantageKit — Full robot state logging to .wpilog files (USB on roboRIO, logs/ locally)
• NetworkTables — Real-time telemetry to AdvantageScope / Elastic dashboard (optional, controlled by enableNtTelemetry)
• SignalLogger — CTRE Phoenix 6 native signal logging for swerve module states
• PowerMonitor — Aggregated per-subsystem current and power draw under Power/Subsystems/*

Testing

JUnit 5 with autodetection enabled. Desktop simulation support (includeDesktopSupport = true) allows IO sim classes to run without hardware.

./gradlew test

Contributing

1. Create your own branch from master
2. Make changes and test locally (./gradlew simRun or ./gradlew test)
3. Open a Pull Request to master — never push directly to master

Acknowledgments

This project includes utility code from several FRC teams:

• Team 95 — Swerve kinematics improvements
• Team 1323 — HSV color conversion, moving average
• Team 1678 — CTRE module state optimization, conversions
• Team 1706 — Field-relative speed/acceleration, linear interpolation
• Team 2910 — Math utilities, PID control, interpolating maps
• Team 3476 — Timestamped pose tracking, live-editable values
• Team 503 — Interpolating tree maps
• Team 6328 — Motion-compensated ballistics solver
• Team 6940 — Trajectory utilities

License

This project is licensed under the WPILib License (BSD-style). See WPILib-License.md.

AdvantageKit is licensed under GPL-3.0. See AdvantageKit-License.md.