### Field Coordinate Constants in Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Defines field-specific locations as Pose2d objects (meters, meters, rotation) for game elements like the speaker and amp for both blue and red alliances. Also includes an enum for auto starting positions with alliance-aware pose selection using DriverStation.

```java
// All field positions in Pose2d (meters, meters, rotation)
public static class Field {
    // Blue alliance targets
    public static final Pose2d BLUE_SPEAKER = new Pose2d(
        Units.inchesToMeters(-1.5),
        Units.inchesToMeters(218.42),
        new Rotation2d()
    );
    public static final Pose2d BLUE_AMP = new Pose2d(
        Units.inchesToMeters(72.5),
        Units.inchesToMeters(323.0),
        Rotation2d.fromDegrees(270)
    );

    // Red alliance targets (mirrored)
    public static final Pose2d RED_SPEAKER = new Pose2d(
        Units.inchesToMeters(625.7),
        Units.inchesToMeters(218.42),
        new Rotation2d()
    );

    // Stage positions for climbing
    public static final Pose2d BLUE_STAGE_ONE = ...;

    // Podium shooting positions
    public static final Pose2d BLUE_PODIUM = ...;
}

// Auto starting positions with alliance-aware selection
public enum AUTOPOSES {
    AMP(redPose, bluePose),
    MIDDLE(redPose, bluePose),
    SOURCE(redPose, bluePose);

    public Pose2d getPose() {
        return (DriverStation.getAlliance().get() == Alliance.Red)
            ? _redPose : _bluePose;
    }
}
```

--------------------------------

### Operating Mode Configuration in Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Configures the robot's operating mode (REAL, SIM, REPLAY) using an enum and AdvantageKit. Demonstrates how the Drive class initializes different IO implementations based on the selected mode. Includes details on logged data paths for each mode.

```java
public static class AdvantageKit {
    public static final Mode CURRENT_MODE = Mode.SIM;

    public static enum Mode {
        REAL,    // Running on physical RoboRIO
        SIM,     // Physics simulation
        REPLAY   // Replaying from AdvantageKit log
    }
}

// RobotContainer automatically selects IO implementations:
switch (Constants.AdvantageKit.CURRENT_MODE) {
    case REAL:
        _drive = new Drive(
            _gyro,
            new ModuleIOHardware(CAN.MODULE_FL_DRIVE, CAN.MODULE_FL_ROTATE, ...),
            // ... other hardware IOs
        );
        break;
    case SIM:
        _drive = new Drive(
            _gyro,
            new ModuleIOSim(),
            // ... other sim IOs
        );
        break;
    case REPLAY:
        _drive = new Drive(
            _gyro,
            new ModuleIO() {},  // Empty interface = replay data only
            // ... other replay IOs
        );
        break;
}

// Logged data paths in AdvantageKit:
// - "/U/logs" on real robot (USB stick)
// - NetworkTables for simulation
// - Replay source from WPILOGReader
```

--------------------------------

### Control Notepath State with Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Demonstrates how to initialize and control the notepath subsystem in Java. It covers setting different operating modes, checking sensor status for note detection, and manually tracking note presence. This subsystem is crucial for managing the internal conveyor.

```java
// Initialize notepath (internal conveyor) subsystem
Notepath notepath = new Notepath(new NotepathIOSparkMax());

// Set notepath to different operating modes
notepath.set(NotepathState.IntakeLoad);   // 15% output for intake pickup
notepath.set(NotepathState.Feed);         // 60% output for shooting
notepath.set(NotepathState.ShooterLoad);  // 30% output for shooter pickup
notepath.set(NotepathState.Off);          // Stop conveyor

// Sensor-based note detection
boolean hasNote = notepath.sensorTripped(); // Digital sensor on DIO pin 1

// Manual note tracking flag
notepath.setHasNote(true);   // Mark note loaded
boolean notePresent = notepath.hasNote();

// Example sequence: Load note from intake
Command loadSequence = Commands.sequence(
    NotepathCommands.intakeLoad(notepath),
    Commands.waitUntil(() -> notepath.sensorTripped()),
    NotepathCommands.stop(notepath)
);

// Constants used:
// - IntakeLoad: 1.8V (15% of 12V)
// - Feed: 7.2V (60% of 12V)
// - ShooterLoad: 3.6V (30% of 12V)
```

--------------------------------

### Basic LED Control with Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Initializes an LED subsystem connected via PWM and provides commands to set LED colors and patterns. It supports setting all LEDs to a solid color, defining custom patterns for individual LEDs, and creating pulsing color effects. Predefined colors are available for common states.

```java
// Initialize LED subsystem (6 LEDs on PWM)
LED led = new LED(new LEDIOHardware());

// Set all LEDs to solid color
Command solidOrange = CompositeCommands.Teleop.LEDSetSolidColor(led, Constants.LED.ORANGE);
led.setDefaultCommand(solidOrange);

// Predefined colors: 
// RED, BLUE, ORANGE, PURPLE, GREEN, PINK, TEAL, WHITE, OFF, GREY

// Set individual LED colors programmatically
Color[] customPattern = new Color[Constants.LED.NUM_LEDS];
Arrays.fill(customPattern, Constants.LED.GREEN);
customPattern[0] = Constants.LED.RED;  // First LED red
Command customFrame = LEDCommands.setFrame(led, customPattern);

// Pulse color effect (breathing animation)
Command pulseGreen = CompositeCommands.Teleop.LEDPulseColor(led, Constants.LED.GREEN);
controller.rightStick().whileTrue(pulseGreen);

// Animation cycles through 32 brightness levels (0-100%)
// Refresh rate: Command scheduler periodic (50Hz)
```

--------------------------------

### Initialize and Control Shooter Bed Angle

Source: https://context7.com/swartdogs/crescendocode/llms.txt

This section covers the initialization of the shooter bed subsystem using a VictorSPX controller and controlling its angle using predefined enum values or manual voltage control. It includes checks for reaching the setpoint, retrieving the current angle, and highlights the safety limits and voltage considerations for upward and downward movement.

```java
// Initialize shooter bed subsystem
ShooterBed shooterBed = new ShooterBed(new ShooterBedIOVictorSPX());

// Set bed to predefined angles using enum
shooterBed.setAngle(BedAngle.SubwooferShot);  // 54.5°
shooterBed.setAngle(BedAngle.PodiumShot);     // 31°
shooterBed.setAngle(BedAngle.AmpShot);        // 50°
shooterBed.setAngle(BedAngle.IntakeLoad);     // 65.1°
shooterBed.setAngle(BedAngle.Stow);           // 37°

// Manual voltage control for operator adjustment
shooterBed.setVolts(3.0); // Move up at 3V

// Check if at target angle (within tolerance)
boolean ready = shooterBed.atSetpoint();

// Get current angle
Rotation2d currentAngle = shooterBed.getBedAngle();

// Safety limits enforced:
// - Min angle: 24°
// - Max angle: 96°
// - Different min voltages for up (-2.3V) vs down (-3.5V) to overcome gravity
```

--------------------------------

### Initialize and Control Intake Subsystem

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Demonstrates how to initialize the intake subsystem using a SparkMax controller and control its state (On, Reverse, Off). It also shows how to set custom intake speeds and check if the intake is currently running. The default intake speed is 9.6V (80% of 12V) and outtake is 7.2V (60% of 12V).

```java
// Initialize intake subsystem
Intake intake = new Intake(new IntakeIOSparkMax());

// Start intake to collect game piece
intake.set(IntakeState.On);

// Reverse intake to eject game piece
intake.set(IntakeState.Reverse);

// Stop intake motor
intake.set(IntakeState.Off);

// Check if currently intaking
boolean isRunning = intake.isIntaking(); // Returns true if motorVolts > 0

// Custom intake speed control (0.0 to 1.0)
intake.setInSpeed(0.8);  // 80% of 12V = 9.6V
intake.set(IntakeState.On);

// Expected behavior:
// - Default intake: 9.6V (80% of 12V battery)
// - Default outtake: 7.2V (60% of 12V battery)
// - Logged to AdvantageKit: "Intake/motorVolts", "Intake/motorCurrent"
```

--------------------------------

### Holonomic Path Following Configuration

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Configures the holonomic drive subsystem for path following. It requires suppliers for the current pose, a consumer to set the pose, suppliers for current chassis speeds, and a consumer to run velocity commands. It also takes a HolonomicPathFollowerConfig and an alliance supplier. The configuration is mirrored for the red alliance.

```java
// PathPlanner automatically configured in Drive constructor
AutoBuilder.configureHolonomic(
    this::getPose,              // Current pose supplier
    this::setPose,              // Pose reset consumer
    this::getChassisSpeeds,     // Current speeds supplier
    this::runVelocity,          // Drive command consumer
    new HolonomicPathFollowerConfig(
        Constants.Drive.MAX_LINEAR_SPEED,      // Max velocity: 4.22 m/s
        Constants.Drive.DRIVE_BASE_RADIUS,     // Base radius: 0.38m
        new ReplanningConfig()                 // Enable dynamic replanning
    ),
    () -> DriverStation.getAlliance().get() == Alliance.Red, // Mirror for red alliance
    drive
);

// Load and follow pre-built autonomous path
Command followPath = AutoBuilder.followPath(
    PathPlannerPath.fromPathFile("2NoteMiddleAuto")
);

// Build path on-the-fly to target pose
Command pathToAmp = AutoBuilder.pathfindToPose(
    Constants.Field.BLUE_AMP,
    new PathConstraints(3.0, 3.0, Math.PI, 2*Math.PI),
    0.0  // Goal end velocity
);

// Logged to AdvantageKit:
// - "Odometry/Trajectory": Full path visualization
// - "Odometry/TrajectorySetpoint": Current target pose
```

--------------------------------

### Complex Autonomous Routine with Path Following

Source: https://context7.com/swartdogs/crescendocode/llms.txt

A multi-note autonomous routine that sequences path following and game actions. It includes setting the initial pose, shooting a preloaded note, driving to a center field note while preparing for intake, picking up the second note, returning to a shooting position, and shooting the second note. This routine is available in the dashboard autonomous chooser.

```java
// Multi-note autonomous combining path following and game actions
Command twoNoteAuto = Commands.sequence(
    // Start position: Center subwoofer
    Commands.runOnce(() -> drive.setPose(Constants.AUTOPOSES.MIDDLE.getPose())),

    // Shoot preloaded note
    ShooterBedCommands.setAngle(shooterBed, BedAngle.SubwooferShot),
    ShooterFlywheelCommands.start(shooterFlywheel, 4000, 4000),
    Commands.waitUntil(() -> shooterFlywheel.atSpeed()),
    CompositeCommands.Autonomous.startNotepath(notepath),

    // Drive to center field note while preparing intake
    Commands.deadline(
        AutoBuilder.followPath(PathPlannerPath.fromPathFile("MiddleToCenter1")),
        ShooterBedCommands.setAngle(shooterBed, BedAngle.IntakeLoad)
    ),

    // Pick up second note
    CompositeCommands.Autonomous.intakePickup(intake, notepath, shooterBed),

    // Return to shooting position
    AutoBuilder.pathfindToPose(
        Constants.AUTOPOSES.MIDDLE.getPose(),
        new PathConstraints(4.0, 4.0, Math.PI, 2*Math.PI),
        0.0
    ),

    // Shoot second note
    CompositeCommands.Autonomous.setBedAngle(shooterBed, BedAngle.SubwooferShot),
    CompositeCommands.Autonomous.startShooter(shooterFlywheel, 4000, 4000),
    CompositeCommands.Autonomous.startNotepath(notepath)
);

// Available in Dashboard autonomous chooser
// PID tuning: Translation (5.0, 0, 0), Rotation (5.0, 0, 0)
```

--------------------------------

### Initialize and Control Shooter Flywheel Velocity

Source: https://context7.com/swartdogs/crescendocode/llms.txt

This code initializes the shooter flywheel subsystem using a SparkMax controller and demonstrates setting independent velocities for the upper and lower flywheels in RPM. It includes checks for achieving target speed, stopping the flywheels, and configuring intake mode. Configuration options for max speed and velocity tolerance are also shown.

```java
// Initialize shooter flywheel subsystem
ShooterFlywheel shooterFlywheel = new ShooterFlywheel(new ShooterFlywheelIOSparkMax());

// Set independent upper and lower flywheel velocities (RPM)
shooterFlywheel.setVelocity(4000, 4000); // Subwoofer shot

// Different velocities for spin shots
shooterFlywheel.setVelocity(5500, 5000); // Podium shot with backspin

// Check if at target velocity (within 15% tolerance)
if (shooterFlywheel.atSpeed()) {
    System.out.println("Ready to shoot!");
}

// Stop both flywheels
shooterFlywheel.stop();

// Intake mode (reverse rotation at 1.8V)
shooterFlywheel.intake();

// Configuration options:
shooterFlywheel.setMaxSpeed(0.9);        // Limit to 90% of NEO max (5286 RPM)
shooterFlywheel.setVelocityRange(0.10);  // Tighten tolerance to 10%

// Logged outputs: "Shooter/Flywheel/UpperVelocitySetpoint", "Shooter/Flywheel/upperVelocity"
```

--------------------------------

### Game State LED Feedback with Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Implements LED feedback based on the robot's current game state. This includes displaying the alliance color during autonomous, indicating if a note is detected, showing shooting or intaking status with pulsing effects, and setting a default color for disabled mode. These states provide instant visual cues for robot operations.

```java
// Autonomous mode: Show alliance color
Command autoLEDs = CompositeCommands.Teleop.LEDAutonomous(led);
// RED for red alliance stations, BLUE for blue alliance, ORANGE if unknown

// Note detection triggers
Trigger hasNote = new Trigger(() -> notepath.hasNote());
hasNote.onTrue(CompositeCommands.Teleop.LEDSetDefaultColor(led, Constants.LED.GREEN));
hasNote.onFalse(CompositeCommands.Teleop.LEDSetDefaultColor(led, Constants.LED.RED));

// Shooter state indication
Trigger shooting = new Trigger(() -> shooterFlywheel.isShooting());
shooting.whileTrue(CompositeCommands.Teleop.LEDPulseColor(led, Constants.LED.GREEN));

Trigger intaking = new Trigger(() -> intake.isIntaking());
intaking.whileTrue(CompositeCommands.Teleop.LEDPulseColor(led, Constants.LED.RED));

// Disabled mode
Commands.runOnce(() -> led.switchDefaultCommand(
    CompositeCommands.Teleop.LEDSetSolidColor(led, Constants.LED.ORANGE)
)).ignoringDisable(true);

// LED states provide instant visual feedback:
// - ORANGE: Default/Disabled
// - GREEN: Note loaded
// - RED: No note
// - Pulsing GREEN: Shooting
// - Pulsing RED: Intaking
```

--------------------------------

### Autonomous Game Piece Handling Workflow with Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Implements a complete autonomous workflow for picking up and shooting a note using multiple robot subsystems. This sequence includes intaking from the ground, stowing components, aiming at the speaker, and feeding the note. It relies on sensors for confirmation at critical steps.

```java
// Full autonomous note pickup and shoot sequence
Command autoShootNote = Commands.sequence(
    // Step 1: Intake from ground
    ShooterBedCommands.setAngle(shooterBed, BedAngle.IntakeLoad),
    Commands.parallel(
        IntakeCommands.start(intake),
        NotepathCommands.intakeLoad(notepath)
    ),
    Commands.waitUntil(() -> notepath.sensorTripped()),

    // Step 2: Stop intake and stow
    Commands.runOnce(() -> {
        intake.set(IntakeState.Off);
        notepath.set(NotepathState.Off);
    }),
    ShooterBedCommands.setAngle(shooterBed, BedAngle.Stow),

    // Step 3: Aim and shoot
    DriveCommands.aimAtSpeaker(drive, dashboard, xSupplier, ySupplier, robotCentric, 0.6),
    ShooterFlywheelCommands.start(shooterFlywheel, 4000, 4000),
    Commands.waitUntil(() -> shooterFlywheel.atSpeed()),

    // Step 4: Feed and shoot
    NotepathCommands.feed(notepath),
    Commands.waitUntil(() -> !notepath.sensorTripped()),
    Commands.runOnce(() -> notepath.setHasNote(false))
);

// Total execution time: ~3-5 seconds
// Sensor confirms note presence at each critical step
```

--------------------------------

### Hardware CAN Bus Mapping in Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Defines static final integers for hardware CAN bus IDs, organized by subsystem (Swerve drive modules, game piece manipulation, climbing). Includes analog input sensor mappings and swerve module calibration offsets. Assumes 'Constants.java' for global constants and 'Rotation2d' for rotational data.

```java
public static class CAN {
    // Swerve drive modules (FL, FR, BL, BR)
    public static final int MODULE_FL_DRIVE = 1;
    public static final int MODULE_FL_ROTATE = 2;
    // ... FR: 3,4  BL: 5,6  BR: 7,8

    // Game piece manipulation
    public static final int INTAKE = 9;
    public static final int NOTEPATH_LEADER = 11;
    public static final int NOTEPATH_FOLLOWER = 12;
    public static final int SHOOTER_BED_LEADER = 13;
    public static final int SHOOTER_BED_FOLLOWER = 14;
    public static final int SHOOTER_FLYWHEEL_LOWER = 15;
    public static final int SHOOTER_FLYWHEEL_UPPER = 16;

    // Climbing mechanism
    public static final int CLIMB_LEFT = 17;
    public static final int CLIMB_RIGHT = 18;
}

// Analog input sensors
public static class AIO {
    public static final int MODULE_FL_SENSOR = 0;
    public static final int NOTE_SENSOR = 1;  // DIO actually
}

// Swerve module calibration offsets (radians)
public static final Rotation2d MODULE_FL_OFFSET = Rotation2d.fromRadians(-2.49).plus(Rotation2d.fromDegrees(180));
// Additional offsets for FR, BL, BR modules...
```

--------------------------------

### Vision-Assisted Speaker Targeting with Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Utilizes a vision subsystem with a PhotonVision camera for automatic targeting of the AprilTag speaker. This code snippet shows how to initialize the vision system, enable targeting, check for target visibility, and obtain rotation speeds for aiming. It integrates with the drive subsystem for precise movement.

```java
// Initialize vision subsystem with PhotonVision camera
Vision vision = new Vision(drive, new VisionIOPhotonlib(drive));

// Enable targeting mode
vision.setTargetingEnabled(true);

// Check if AprilTag speaker target is visible
if (vision.seesSpeaker()) {
    // Get rotation speed to aim at speaker (PID controlled)
    double rotationSpeed = vision.rotateExecute();

    // Apply to drive while allowing driver translation control
    ChassisSpeeds speeds = new ChassisSpeeds(
        xSpeed,         // Driver X control
        ySpeed,         // Driver Y control
        rotationSpeed   // Automatic rotation to target
    );
    drive.runVelocity(speeds);
}

// Disable targeting when done
vision.setTargetingEnabled(false);

// Camera configuration:
// - Camera name: "photonCam"
// - Transform: 7" left, 7" back, 9.5" up, tilted -33°
// - Max detection range: 300 inches
// - Alignment offset: -3° for shooter offset
```

--------------------------------

### Automated Intake Pickup Command

Source: https://context7.com/swartdogs/crescendocode/llms.txt

This snippet shows how to create and bind a composite command for an automated intake pickup sequence. This command integrates shooter bed positioning, intake motor activation, notepath conveyor control, and note detection. It includes safety checks and cleanup actions to ensure reliable operation.

```java
// Complete intake sequence with shooter bed positioning and note detection
Command intakePickup = CompositeCommands.Teleop.intakePickup(
    intake,
    notepath,
    shooterBed,
    controller.getHID()
);

// Bind to controller button
controller.a().onTrue(intakePickup);

// Command execution sequence:
// 1. Move shooter bed to IntakeLoad angle (65.1°)
// 2. Start intake motor at 9.6V
// 3. Run notepath conveyor at 15% output
// 4. Wait for sensor to detect note (notepath.sensorTripped())
// 5. Stow shooter bed to 37° angle
// 6. Rumble controller for 0.5 seconds
// 7. Stop all motors and set hasNote flag

// Safety: Command skips if notepath.hasNote() is already true
// Cleanup: Motors stop even if command interrupted
```

--------------------------------

### Pose Estimation and Odometry with Vision Correction (Java)

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Provides real-time robot pose estimation using swerve kinematics, gyro data, and vision measurements. It allows for retrieving the current pose, resetting the pose to a known position, and incorporating vision data for improved accuracy. Dependencies include the Drive subsystem and potentially vision systems like PhotonVision. Outputs are Pose2d objects and module states/positions.

```java
// Get current robot pose (updated via swerve kinematics + gyro)
Pose2d currentPose = drive.getPose();
System.out.println("X: " + currentPose.getX() + " Y: " + currentPose.getY());
System.out.println("Heading: " + currentPose.getRotation().getDegrees() + "°");

// Reset pose to known position (e.g., at start of auto)
Pose2d startPose = Constants.AUTOPOSES.MIDDLE.getPose();
drive.setPose(startPose);

// Add vision measurement for pose correction
drive.addVisionMeasurement(visionPose, timestamp);

// Get current module positions and states
SwerveModulePosition[] positions = drive.getModulePositions();
SwerveModuleState[] states = drive.getModuleStates();

// Example output:
// [FL: 1.23m @ 45°, FR: 1.24m @ 135°, BL: 1.22m @ -45°, BR: 1.25m @ -135°]
// Logged to AdvantageKit under "Odometry/Robot" and "SwerveStates/Measured"
```

--------------------------------

### Intake System API

Source: https://context7.com/swartdogs/crescendocode/llms.txt

API for controlling the robot's intake mechanism to collect and manage game pieces.

--------------------------------

### Complete Shooting Sequence Command

Source: https://context7.com/swartdogs/crescendocode/llms.txt

This composite command orchestrates a full shooting sequence, including setting the shooter bed angle, spinning up the flywheels, feeding the note, and then stowing the bed. It uses `Commands.sequence` and `Commands.waitUntil` to manage the timing and dependencies between actions. The `finallyDo` block ensures cleanup actions like stopping motors occur regardless of command interruption.

```java
// Composite command for full shooting operation
Command shoot = Commands.sequence(
    // Set bed angle to subwoofer shot position
    ShooterBedCommands.setAngle(shooterBed, BedAngle.SubwooferShot),

    // Spin up flywheels to 4000 RPM
    ShooterFlywheelCommands.start(shooterFlywheel, 4000, 4000),

    // Wait for flywheels to reach target speed
    Commands.waitUntil(() -> shooterFlywheel.atSpeed()),

    // Feed note through notepath conveyor
    NotepathCommands.feed(notepath),

    // Wait for note to exit (sensor no longer tripped)
    Commands.waitUntil(() -> !notepath.sensorTripped()),

    // Stow bed and stop flywheels
    ShooterBedCommands.setAngle(shooterBed, BedAngle.Stow)
).finallyDo(() -> {
    notepath.set(NotepathState.Off);
    shooterFlywheel.stop();
    notepath.setHasNote(false);
});

// Bind to controller POV button
controller.povDown().onTrue(shoot);

// Execution time: ~1-2 seconds from trigger to note exit
```

--------------------------------

### Vision-Assisted Drive Command with Java

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Integrates vision targeting with joystick control for a seamless teleoperated driving experience. This command allows the driver to maintain full translation control while the robot automatically rotates to aim at the speaker when a target is visible. It includes fallback to manual rotation and speed reduction for precision.

```java
// Complete command integrating vision with joystick control
Command visionAimAndDrive = CompositeCommands.Teleop.visionAimAtSpeaker(
    drive,
    vision,
    () -> -joystick.getY(),  // Forward/back control
    () -> -joystick.getX(),  // Left/right control
    () -> -joystick.getZ(),  // Manual rotation (if no target)
    () -> false,             // Field-relative mode
    dashboard
);

// Bind to joystick button
joystick.button(11).whileTrue(visionAimAndDrive);

// Behavior:
// - Driver maintains full translation control via joystick
// - When speaker AprilTag visible: automatic rotation to target
// - When no target: falls back to manual rotation control
// - Speed reduced to 20% for precise aiming
// - Targeting disabled automatically when command ends

// Vision pose correction (if enabled):
if (Constants.Vision.ENABLE_POSE_CORRECTION) {
    Pose2d visionPose = vision.getEstimatedPose();
    drive.addVisionMeasurement(visionPose, vision.getTimestamp());
}
```

--------------------------------

### Joystick Control for Swerve Drive with Field-Relative Mode (Java)

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Configures and applies joystick inputs to control the swerve drive subsystem. It allows for forward/backward, left/right, and rotational control, with an option to switch to robot-centric mode. Dependencies include the Drive subsystem, RobotContainer, and Dashboard. Inputs are joystick axes and buttons, with outputs being robot movement. Limitations include reliance on joystick input and specific button mappings.

```java
RobotContainer robotContainer = new RobotContainer();
Drive drive = robotContainer.getDrive();
Dashboard dashboard = robotContainer.getDashboard();

// Create joystick drive command with exponential response curves
Command teleopDrive = DriveCommands.joystickDrive(
    drive,
    () -> -joystick.getY(),           // Forward/backward supplier
    () -> -joystick.getX(),           // Left/right supplier
    () -> -joystick.getZ(),           // Rotation supplier
    () -> joystick.button(3).getAsBoolean(), // Robot-centric mode
    dashboard
);

// Set as default command - runs continuously during teleop
drive.setDefaultCommand(teleopDrive);

// Example output during operation:
// - Robot moves at up to 13.86 ft/s linear speed
// - Rotation up to 4.9 rad/s angular velocity
// - Automatic field-relative orientation flipping for red alliance
// - 0.1 joystick deadband with squared response curves
```

--------------------------------

### Drive System API

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Control and manage the swerve drive system for robot navigation and movement.

```APIDOC
## Joystick Control with Field-Relative Drive

### Description
Enables teleoperated control of the swerve drive using joystick input, with options for field-relative or robot-centric control and exponential response curves.

### Method
Command-based execution within the robot's teleoperation period.

### Endpoint
N/A (Internal command execution)

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
None (Uses joystick input and robot container configuration)

### Request Example
```java
// Initialize robot container which sets up the drive subsystem
RobotContainer robotContainer = new RobotContainer();
Drive drive = robotContainer.getDrive();
Dashboard dashboard = robotContainer.getDashboard();

// Create joystick drive command with exponential response curves
Command teleopDrive = DriveCommands.joystickDrive(
    drive,
    () -> -joystick.getY(),           // Forward/backward supplier
    () -> -joystick.getX(),           // Left/right supplier
    () -> -joystick.getZ(),           // Rotation supplier
    () -> joystick.button(3).getAsBoolean(), // Robot-centric mode
    dashboard
);

// Set as default command - runs continuously during teleop
drive.setDefaultCommand(teleopDrive);
```

### Response
#### Success Response (200)
Command runs continuously, updating drive system based on joystick input.

#### Response Example
```
// Example output during operation:
// - Robot moves at up to 13.86 ft/s linear speed
// - Rotation up to 4.9 rad/s angular velocity
// - Automatic field-relative orientation flipping for red alliance
// - 0.1 joystick deadband with squared response curves
```
```

```APIDOC
## Swerve Drive Velocity Control

### Description
Allows direct control of the swerve drive system by setting chassis velocities (linear and angular) for precise autonomous or manual movements.

### Method
`drive.runVelocity(ChassisSpeeds speeds)`

### Endpoint
N/A (Internal method call)

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
None (Parameters are passed directly to the `runVelocity` method)

### Request Example
```java
// Run drive at specific chassis speeds (meters/second and radians/second)
ChassisSpeeds speeds = new ChassisSpeeds(
    2.0,    // vx: 2 m/s forward
    1.0,    // vy: 1 m/s left
    0.5     // omega: 0.5 rad/s counterclockwise
);
drive.runVelocity(speeds);

// Field-relative driving from current pose
ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
    3.0,                    // Field-relative X velocity
    0.0,                    // Field-relative Y velocity
    1.0,                    // Rotation velocity
    drive.getRotation()     // Current robot heading
);
drive.runVelocity(fieldRelativeSpeeds);

// Stop the drive immediately
drive.stop();
```

### Response
#### Success Response (200)
Drive system updates to the specified velocities or stops.

#### Response Example
```
// Expected behavior:
// - Modules optimize to shortest path (max 90° rotation)
// - Wheel speeds desaturated to respect physical limits
// - Logged to AdvantageKit: "SwerveStates/Setpoints" and "SwerveStates/SetpointsOptimized"
```
```

```APIDOC
## Autonomous Rotation to Target Angle

### Description
Provides functionality to autonomously rotate the robot to face a specific target angle, such as a game element or field position, using a PID controller.

### Method
`drive.rotateInit(targetAngle, maxSpeed)` and `drive.rotateExecute()`

### Endpoint
N/A (Internal command execution)

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
None

### Request Example
```java
// Initialize PID rotation to face speaker
double speakerHeading = getHeadingToPose(drive, Constants.Field.BLUE_SPEAKER);
drive.rotateInit(speakerHeading, 0.6); // 60% max rotation speed

// Execute in command (call periodically)
Command aimAtSpeaker = Commands.run(() -> {
    double rotationSpeed = drive.rotateExecute();
    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotationSpeed);
    drive.runVelocity(speeds);
}, drive);

// Alternative: Aim while driving
Command driveAndAim = DriveCommands.aimAtSpeaker(
    drive,
    dashboard,
    () -> -joystick.getY(),
    () -> -joystick.getX(),
    () -> false, // Field-relative
    0.6          // Max rotation speed
);
```

### Response
#### Success Response (200)
Robot rotates towards the target angle, with `rotateExecute` returning the calculated rotation speed.

#### Response Example
```
// PID Controller configuration:
// - Kp: 0.026, Ki: 0, Kd: 0
// - Continuous input: -180° to 180°
// - Output clamped to maxSpeed parameter
```
```

```APIDOC
## Pose Estimation and Odometry

### Description
Manages the robot's position and orientation on the field using sensor data (IMU, encoders) and vision measurements, providing methods to retrieve and reset the robot's pose.

### Method
`drive.getPose()`, `drive.setPose(Pose2d pose)`, `drive.addVisionMeasurement(Pose2d pose, double timestamp)`

### Endpoint
N/A (Internal method calls)

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
None

### Request Example
```java
// Get current robot pose (updated via swerve kinematics + gyro)
Pose2d currentPose = drive.getPose();
System.out.println("X: " + currentPose.getX() + " Y: " + currentPose.getY());
System.out.println("Heading: " + currentPose.getRotation().getDegrees() + "°");

// Reset pose to known position (e.g., at start of auto)
Pose2d startPose = Constants.AUTOPOSES.MIDDLE.getPose();
drive.setPose(startPose);

// Add vision measurement for pose correction
drive.addVisionMeasurement(visionPose, timestamp);

// Get current module positions and states
SwerveModulePosition[] positions = drive.getModulePositions();
SwerveModuleState[] states = drive.getModuleStates();
```

### Response
#### Success Response (200)
Returns the current robot pose, allows resetting the pose, and incorporates vision measurements.

#### Response Example
```
// Example output:
// [FL: 1.23m @ 45°, FR: 1.24m @ 135°, BL: 1.22m @ -45°, BR: 1.25m @ -135°]
// Logged to AdvantageKit under "Odometry/Robot" and "SwerveStates/Measured"
```
```

--------------------------------

### Autonomous Rotation to Target Angle (Java)

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Enables autonomous rotation of the swerve drive to face a specific target angle, such as the speaker. It utilizes a PID controller for precise rotation and can be integrated into autonomous commands or combined with driving commands. Inputs include a target heading and max rotation speed, with output being rotation commands. PID controller parameters are configurable.

```java
// Initialize PID rotation to face speaker
double speakerHeading = getHeadingToPose(drive, Constants.Field.BLUE_SPEAKER);
drive.rotateInit(speakerHeading, 0.6); // 60% max rotation speed

// Execute in command (call periodically)
Command aimAtSpeaker = Commands.run(() -> {
    double rotationSpeed = drive.rotateExecute();
    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotationSpeed);
    drive.runVelocity(speeds);
}, drive);

// Alternative: Aim while driving
Command driveAndAim = DriveCommands.aimAtSpeaker(
    drive,
    dashboard,
    () -> -joystick.getY(),
    () -> -joystick.getX(),
    () -> false, // Field-relative
    0.6          // Max rotation speed
);

// PID Controller configuration:
// - Kp: 0.026, Ki: 0, Kd: 0
// - Continuous input: -180° to 180°
// - Output clamped to maxSpeed parameter
```

--------------------------------

### Swerve Drive Velocity Control (Java)

Source: https://context7.com/swartdogs/crescendocode/llms.txt

Directly controls the swerve drive's velocity using ChassisSpeeds. This allows setting linear (vx, vy) and rotational (omega) speeds in both robot-relative and field-relative frames. The system optimizes module paths, desaturates wheel speeds to respect physical limits, and logs data to AdvantageKit. Inputs are ChassisSpeeds objects, and the output is the robot's controlled motion.

```java
// Run drive at specific chassis speeds (meters/second and radians/second)
ChassisSpeeds speeds = new ChassisSpeeds(
    2.0,    // vx: 2 m/s forward
    1.0,    // vy: 1 m/s left
    0.5     // omega: 0.5 rad/s counterclockwise
);
drive.runVelocity(speeds);

// Field-relative driving from current pose
ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
    3.0,                    // Field-relative X velocity
    0.0,                    // Field-relative Y velocity
    1.0,                    // Rotation velocity
    drive.getRotation()     // Current robot heading
);
drive.runVelocity(fieldRelativeSpeeds);

// Stop the drive immediately
drive.stop();

// Expected behavior:
// - Modules optimize to shortest path (max 90° rotation)
// - Wheel speeds desaturated to respect physical limits
// - Logged to AdvantageKit: "SwerveStates/Setpoints" and "SwerveStates/SetpointsOptimized"
```

=== COMPLETE CONTENT === This response contains all available snippets from this library. No additional content exists. Do not make further requests.