ReefCode

https://github.com/swartdogs/reefcode
Admin
ReefCode is FRC Team 525's robot code specifically designed for the 2025 REEFSCAPE game season.
Tokens:7,131
Snippets:31
Trust Score:4.9
License:GPL-3.0
Update:10 months ago
Show doc for...
Tokens:
Raw
### FunnelCommands: Deployment Control Examples (Java)

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

Provides factory methods for funnel deployment operations. Shows examples for dropping the funnel and configuring a trigger to deploy the funnel and lower the elevator simultaneously. These commands interact with funnel and elevator subsystems.

```java
// Drop funnel (full deployment sequence)
Command deployFunnel = FunnelCommands.drop();
deployFunnel.schedule();
// Applies retract voltage for DROP_TIME_SECS (2.0 seconds)

// Example: Full climb sequence
CommandJoystick operator = new CommandJoystick(2);
CommandJoystick driver = new CommandJoystick(0);

// Require both operator and driver confirmation to drop funnel
Trigger hangTrigger = operator.povDown().and(driver.button(4));
hangTrigger.onTrue(
    Commands.sequence(
        FunnelCommands.drop(),                           // Deploy funnel
        ElevatorCommands.setHeight(ElevatorHeight.Hang)  // Lower to hang position
    )
);

// Typical hang procedure
Command fullHangSequence = Commands.sequence(
    ElevatorCommands.setHeight(ElevatorHeight.Level4),  // Extend high
    Commands.waitSeconds(1.0),                           // Position robot
    FunnelCommands.drop(),                               // Drop funnel
    ElevatorCommands.setHeight(ElevatorHeight.Hang),    // Lower onto bar
    Commands.waitSeconds(2.0)
);
```

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

### Custom Gradle Deployment and RoboRIO Configuration

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

Defines a custom Gradle task 'eventDeploy' that automatically commits changes to Git when on branches starting with 'event'. It also configures the deployment process for the RoboRIO, specifying JVM arguments for optimization and deploying necessary artifacts like PathPlanner trajectories. This setup ensures consistent deployment and version control for event-specific branches.

```groovy
// Custom event deploy task automatically commits changes
// When on branches starting with "event", creates timestamped commit
task(eventDeploy) {
    doLast {
        def branchPrefix = "event"
        def branch = 'git branch --show-current'.execute().text.trim()
        def commitMessage = "Update at '${new Date().toString()}'"

        if (branch.startsWith(branchPrefix)) {
            exec { commandLine 'git', 'add', '-A' }
            exec {
                commandLine 'git', 'commit', '-m', commitMessage
                ignoreExitValue = true
            }
            println "Committed to branch: '$branch'"
        }
    }
}

// Deploy artifact configuration for RoboRIO
deploy {
    targets {
        roborio(getTargetTypeClass('RoboRIO')) {
            team = project.frc.getTeamNumber()  // Team 525

            artifacts {
                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
                    // Optimized JVM arguments for RoboRIO
                    jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
                    jvmArgs.add("-XX:GCTimeRatio=5")
                    jvmArgs.add("-XX:+UseSerialGC")
                    jvmArgs.add("-XX:MaxGCPauseMillis=50")
                }

                // Deploy PathPlanner paths and configuration
                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                    files = project.fileTree('src/main/deploy')
                    directory = '/home/lvuser/deploy'
                }
            }
        }
    }
}

```

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

### CompositeCommands: Multi-Subsystem Coordination Examples (Java)

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

Demonstrates complex commands that coordinate multiple subsystems for complete operations. Includes examples for smart intake, full output sequences, setting elevator height, joystick drive with speed scaling, and complete scoring sequences. These commands typically rely on underlying subsystem functionalities.

```java
Command smartIntake = CompositeCommands.fancyIntake();
smartIntake.schedule();
// Repeats intake until coral detected, then runs index command

// Full output sequence (eject coral then stow elevator)
Command completeOutput = CompositeCommands.output();
completeOutput.schedule();
// Runs manipulator output, waits 1 second, then retracts elevator to stow

// Set elevator height and wait for completion
Command moveAndWait = CompositeCommands.setHeight(ElevatorHeight.Level2);
// Blocks until elevator reaches setpoint

// Joystick drive with elevator-based speed scaling
Command scaledDrive = CompositeCommands.joystickDrive(
    () -> -joystick.getY(),
    () -> -joystick.getX(),
    () -> -joystick.getZ(),
    () -> false,  // Field-relative
    2,            // Translation exponent
    5             // Rotation exponent
);
// Speed automatically reduces when elevator extends higher
// At L1 height (27"): 100% speed
// At L3 height (48.5"): ~30% speed

// Example: Complete scoring sequence
Command scoreL3 = Commands.sequence(
    CompositeCommands.setHeight(ElevatorHeight.Level3),  // Extend to L3
    Commands.waitSeconds(0.5),                            // Brief pause
    CompositeCommands.output()                            // Output and retract
);
```

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

### ManipulatorCommands: Intake and Output Operations (Java)

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

Offers factory methods for manipulator control, including intake, indexing, and outputting coral, with automatic stopping conditions based on sensor feedback. Also provides a command to stop the manipulator immediately. Includes examples for binding these to joystick buttons. Dependencies include ManipulatorCommands.

```java
// Intake coral and stop when detected
Command autoIntake = ManipulatorCommands.intake();
autoIntake.schedule();
// This command runs intake() until detectedCoral() returns true

// Index coral backwards slightly (repositioning)
Command indexCoral = ManipulatorCommands.index();
indexCoral.schedule();
// Runs motors at -1V for 0.35 seconds

// Output coral and stop when sensor clears
Command ejectCoral = ManipulatorCommands.output();
ejectCoral.schedule();
// Runs output() until end sensor is no longer tripped

// Stop manipulator immediately
Command stopManip = ManipulatorCommands.stop();
stopManip.schedule();

// Example usage in button bindings
CommandJoystick operatorButtons = new CommandJoystick(2);
operatorButtons.button(6).onTrue(ManipulatorCommands.intake());
operatorButtons.button(7).onTrue(ManipulatorCommands.stop());
operatorButtons.button(8).onTrue(ManipulatorCommands.output());
```

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

### ElevatorCommands: Height Control (Java)

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

Provides factory methods for controlling the elevator's height. Includes commands to move to specific levels, adjust current height incrementally, execute a hang sequence, and provides examples for binding these commands to controller inputs. Dependencies include ElevatorHeight enum and CommandXboxController.

```java
// Move elevator to specific height level
Command moveToL3 = ElevatorCommands.setHeight(ElevatorHeight.Level3);
moveToL3.schedule();

// Modify current height by increment
Command adjustUp = ElevatorCommands.modifyHeight(0.5);  // Add 0.5 inches
Command adjustDown = ElevatorCommands.modifyHeight(-0.5);  // Subtract 0.5 inches

// Execute hang sequence (extends elevator with full power)
Command hangExecute = ElevatorCommands.hangExecute();
// Use: button.whileTrue(hangExecute);

// Example button bindings
CommandXboxController operator = new CommandXboxController(2);
operator.povUp().onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level4));
operator.povRight().onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level3));
operator.povDown().onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level2));
operator.povLeft().onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level1));
operator.back().onTrue(ElevatorCommands.setHeight(ElevatorHeight.Stow));
```

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

### Control Game Piece Manipulator - Java

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

Handles the intake and output of game pieces using motors and sensors. It supports starting and stopping intake, detecting game piece presence, and outputting pieces at appropriate speeds. Manual voltage control and status checks for motor activity and sensor readings are also available.

```java
// Initialize manipulator subsystem
Manipulator manipulator = Manipulator.getInstance();

// Start intake motors
manipulator.intake();

// Check for coral detection (debounced sensor reading)
if (manipulator.hasCoral()) {
    System.out.println("Coral secured!");
    manipulator.stop();
}

// Output coral at appropriate speed based on elevator height
manipulator.output();

// Slow intake for precise positioning
manipulator.slowIntake();

// Check raw sensor states
boolean startSensor = manipulator.isStartSensorTripped();
boolean endSensor = manipulator.isEndSensorTripped();
System.out.println("Start sensor: " + startSensor + ", End sensor: " + endSensor);

// Check if motors are running
if (manipulator.isRunning()) {
    double leftSpeed = manipulator.getLeftOutputSpeed();
    double rightSpeed = manipulator.getRightOutputSpeed();
    System.out.println("Left: " + leftSpeed + ", Right: " + rightSpeed);
}

// Manual voltage control
manipulator.setVolts(8.0);  // Apply 8V to motors
```

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

### Configure Robot Logging in Java

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

Configures the AdvantageKit logging system based on the current robot mode (REAL, SIM, REPLAY). It sets up data receivers like WPILOGWriter and NT4Publisher for different logging destinations, including USB sticks and NetworkTables. For replay mode, it uses WPILOGReader to read existing log files. It also demonstrates how subsystems log their inputs and outputs, such as sensor states, setpoints, and odometry, using Logger.processInputs and Logger.recordOutput.

```java
// Logging is automatically configured in Robot.java based on mode
switch (Constants.AdvantageKit.CURRENT_MODE) {
    case REAL:
        // Running on real robot - log to USB stick and NetworkTables
        Logger.addDataReceiver(new WPILOGWriter());      // Write to /U/logs
        Logger.addDataReceiver(new NT4Publisher());      // Publish to NT4
        break;

    case SIM:
        // Running in simulation - log to NetworkTables only
        Logger.addDataReceiver(new NT4Publisher());
        break;

    case REPLAY:
        // Replaying existing log file
        setUseTiming(false);
        String logPath = LogFileUtil.findReplayLog();
        Logger.setReplaySource(new WPILOGReader(logPath));
        Logger.addDataReceiver(
            new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))
        );
        break;
}

// Subsystems automatically log their inputs via IO layer
// Example from Elevator.periodic():
_io.updateInputs(_inputs);
Logger.processInputs("Elevator", _inputs);  // Logs all inputs
Logger.recordOutput("Setpoint", _extensionSetpoint);  // Log setpoint

// Drive subsystem logs odometry and swerve states
Logger.recordOutput("Odometry/Robot", _poseEstimator.getEstimatedPosition());
Logger.recordOutput("SwerveStates/Measured", getModuleStates());
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);

// Manipulator logs sensor states
Logger.recordOutput("Detected Coral", _coralDetected);
Logger.recordOutput("Has Coral", hasCoral());

// PathPlanner trajectory visualization
PathPlannerLogging.setLogActivePathCallback((activePath) -> {
    Logger.recordOutput("Odometry/Trajectory",
        activePath.toArray(new Pose2d[activePath.size()])
    );
});

PathPlannerLogging.setLogTargetPoseCallback((targetPose) -> {
    Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
});

// Replay logs from command line using Gradle task
// ./gradlew replayWatch
// This watches for new log files and automatically replays them

```

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

### Gradle Tasks for Build and Deployment

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

Provides a list of common Gradle tasks for building, deploying, testing, and simulating the robot code. It includes tasks for standard build processes, deploying to the robot with automatic commit on event branches, running unit tests, simulation, log replay watching, code formatting, and generating version files. These tasks streamline the development workflow.

```bash
# Build the project
./gradlew build

# Deploy to robot (automatically commits on event branches)
./gradlew deploy

# Run unit tests
./gradlew test

# Run in simulation mode
./gradlew simulateJava

# Watch and replay log files
./gradlew replayWatch

# Format code using Spotless
./gradlew spotlessApply

# Generate version file
./gradlew createVersionFile

```

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

### PathPlanner Autonomous Routines Configuration (Java)

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

Details the configuration and usage of PathPlanner for autonomous routines. Includes retrieving commands from the dashboard, outlining available auto routines, configuring the AutoBuilder with robot kinematics, and demonstrating loading and running custom paths or building complex autonomous sequences with inline commands.

```java
// Get autonomous command from dashboard selection
Command autoCommand = Dashboard.getInstance().getSelectedAuto();

// Available autonomous routines (defined in Constants.Autos):
// - LEFT_1_CORAL_AUTO: Starts left side, scores 1 coral (path "Left_J")
// - MIDDLE_1_CORAL_AUTO: Starts middle, scores 1 coral (path "Middle_G")
// - RIGHT_1_CORAL_AUTO: Starts right side, scores 1 coral (path "Right_E")
// - RIGHT_2_CORAL_AUTO: Starts right side, scores 2 corals (path "Right_DC")

// Configure PathPlanner AutoBuilder (done in Drive subsystem constructor)
AutoBuilder.configure(
    this::getPose,                    // Robot pose supplier
    this::setPose,                    // Pose reset consumer
    this::getChassisSpeeds,          // Chassis speeds supplier
    (speeds, ff) -> runVelocity(speeds),  // Output consumer
    new PPHolonomicDriveController(
        new PIDConstants(3.0, 0.0),   // Translation PID (kP=3.0, kD=0.0)
        new PIDConstants(1.5, 0.0)    // Rotation PID (kP=1.5, kD=0.0)
    ),
    robotConfig,
    () -> false,                      // Should flip path (false = blue alliance)
    drive
);

// Use custom pathfinding algorithm
Pathfinding.setPathfinder(new LocalADStarAK());

// Example: Load and run custom auto path
Command customAuto = new PathPlannerAuto("MyCustomPath");
customAuto.schedule();

// Build autonomous with inline commands
Command complexAuto = Commands.sequence(
    DriveCommands.setOdometer(new Pose2d(1.5, 5.5, new Rotation2d())),
    CompositeCommands.fancyIntake(),
    new PathPlannerAuto("ScorePosition1"),
    CompositeCommands.output(),
    new PathPlannerAuto("ReturnToStation")
);
```

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

### Initialize Controllers and Drive Command

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

Initializes various joysticks and button controllers, then sets the default drive command. This command uses a composite command for joystick-based driving with specific scaling for translation and rotation inputs, and it's designed to be elevator-aware.

```java
CommandJoystick driverJoystick = new CommandJoystick(0);   // Main flight stick
CommandJoystick driverButtons = new CommandJoystick(1);    // Button box for driver
CommandJoystick operatorButtons = new CommandJoystick(2);  // Operator button panel

// Set default drive command with elevator-aware speed scaling
Drive.getInstance().setDefaultCommand(
    CompositeCommands.joystickDrive(
        () -> -driverJoystick.getY(),
        () -> -driverJoystick.getX(),
        () -> -driverJoystick.getZ(),
        () -> false,  // Field-relative
        2,            // Square translation inputs
        5             // 5th power rotation inputs
    )
);
```

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

### Robot Configuration Constants (Java)

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

Defines essential system-wide constants for robot operation. This includes drive system parameters like speeds and radii, elevator height presets in inches, field-relative angles for game elements (blue and red alliances), manipulator speeds as fractions of 12V, CAN bus IDs for motor controllers, and control loop parameters.

```java
double MAX_LINEAR_SPEED = Constants.Drive.MAX_LINEAR_SPEED;  // 4.8 m/s
double MAX_ANGULAR_SPEED = Constants.Drive.MAX_ANGULAR_SPEED;
double WHEEL_RADIUS = Constants.Drive.WHEEL_RADIUS;  // 2 inches
double DRIVE_BASE_RADIUS = Constants.Drive.DRIVE_BASE_RADIUS;

double STOW_HEIGHT = Constants.Elevator.STOW_HEIGHT;      // 18.0"
double L1_HEIGHT = Constants.Elevator.L1_HEIGHT;          // 27.0"
double L2_HEIGHT = Constants.Elevator.L2_HEIGHT;          // 33.4"
double L3_HEIGHT = Constants.Elevator.L3_HEIGHT;          // 48.5"
double L4_HEIGHT = Constants.Elevator.L4_HEIGHT;          // 75.5"
double HANG_HEIGHT = Constants.Elevator.HANG_HEIGHT;      // 40.0"

Rotation2d reefAngle1 = Constants.Field.BLUE_REEF_ANGLE_ONE;      // Tag 18
Rotation2d reefAngle2 = Constants.Field.BLUE_REEF_ANGLE_TWO;      // Tag 17
Rotation2d reefAngle3 = Constants.Field.BLUE_REEF_ANGLE_THREE;    // Tag 22
Rotation2d reefAngle4 = Constants.Field.BLUE_REEF_ANGLE_FOUR;     // Tag 21
Rotation2d reefAngle5 = Constants.Field.BLUE_REEF_ANGLE_FIVE;     // Tag 20
Rotation2d reefAngle6 = Constants.Field.BLUE_REEF_ANGLE_SIX;      // Tag 19
Rotation2d leftStation = Constants.Field.BLUE_LEFT_STATION_ANGLE;  // Tag 13 + 180°
Rotation2d rightStation = Constants.Field.BLUE_RIGHT_STATION_ANGLE;// Tag 12 + 180°
Rotation2d processor = Constants.Field.BLUE_PROCESSOR_ANGLE;       // Tag 16

Rotation2d redReef1 = Constants.Field.RED_REEF_ANGLE_ONE;  // Tag 7

double INTAKE_SPEED = Constants.Manipulator.INTAKE_SPEED;          // 3.5/12
double SLOW_INTAKE = Constants.Manipulator.SLOW_INTAKE_SPEED;      // 1.5/12
double OUTPUT_SPEED = Constants.Manipulator.OUTPUT_SPEED;          // 10.0/12

int FL_DRIVE = Constants.CAN.FL_DRIVE;      // 1
int FL_TURN = Constants.CAN.FL_TURN;        // 2
int LEAD_ELEVATOR = Constants.CAN.LEAD_ELEVATOR;        // 9
int FOLLOWER_ELEVATOR = Constants.CAN.FOLLOWER_ELEVATOR;// 10
int MANIPULATOR_LEFT = Constants.CAN.MANIPULATOR_LEFT;  // 11
int MANIPULATOR_RIGHT = Constants.CAN.MANIPULATOR_RIGHT;// 12

double JOYSTICK_DEADBAND = Constants.Controls.JOYSTICK_DEADBAND;  // 0.1
double LOOP_PERIOD_SECS = Constants.General.LOOP_PERIOD_SECS;     // 0.02 (50Hz)
```

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

### Funnel Subsystem Deployment Mechanism (Java)

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

Implements a pneumatic mechanism for dropping the funnel during climbing operations. It initializes the funnel, deploys it by applying voltage to a solenoid, and checks the deployment status. Dependencies include the Funnel class and Constants.Funnel.RETRACT_SPEED.

```java
// Initialize funnel subsystem
Funnel funnel = Funnel.getInstance();

// Deploy funnel (applies voltage to solenoid)
funnel.setVolts(Constants.Funnel.RETRACT_SPEED);

// Check deployment status
if (funnel.isDropped()) {
    System.out.println("Funnel has been deployed");
}

// Typical usage in hang sequence
Command hangSequence = Commands.sequence(
    FunnelCommands.drop(),  // Deploy funnel
    ElevatorCommands.setHeight(ElevatorHeight.Hang),  // Lower elevator
    Commands.waitSeconds(2.0)
);
```

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

### DriveCommands: Joystick and Autonomous Control (Java)

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

Provides factory methods for creating drive-related commands using joystick input. Supports field-relative and robot-centric modes, custom response curves, snapping to orientation, speed reduction, and setting odometry. Relies on a controller object and Constants for configuration.

```java
// Create field-relative joystick drive command
Command teleopDrive = DriveCommands.joystickDrive(
    () -> -controller.getLeftY(),    // Forward/backward
    () -> -controller.getLeftX(),    // Left/right strafe
    () -> -controller.getRightX(),   // Rotation
    () -> false                       // Robot-centric mode (false = field-relative)
);

// Advanced joystick drive with custom response curves
Command customDrive = DriveCommands.joystickDrive(
    () -> -joystick.getY(),
    () -> -joystick.getX(),
    () -> -joystick.getZ(),
    () -> false,
    2,    // Translation exponent (2 = square inputs)
    3     // Rotation exponent (3 = cubic rotation)
);

// Drive while maintaining specific orientation (snap to angle)
Command snapToReef = DriveCommands.driveAtOrientation(
    () -> -joystick.getY(),
    () -> -joystick.getX(),
    () -> false,
    () -> Constants.Field.BLUE_REEF_ANGLE_ONE,  // Target angle supplier
    Constants.Drive.MAX_SNAP_SPEED_PERCENTAGE    // Max rotation speed (0.7)
);

// Reset gyro heading to zero
Command resetHeading = DriveCommands.resetGyro();
resetHeading.schedule();

// Reduce speed temporarily (20% speed)
Command slowMode = DriveCommands.reduceSpeed();
// Bind to button: button.whileTrue(slowMode);

// Set odometry to specific pose
Command setPosition = DriveCommands.setOdometer(
    new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(90))
);
```

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

### Driver Controls for Scoring Positions

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

Configures driver button mappings to snap the robot to specific reef and station angles. This uses a command that drives the robot to a predefined orientation, controlled by individual buttons on the driver's button box.

```java
// Driver controls - Reef scoring positions (snap to angles)
driverButtons.button(1).whileTrue(
    DriveCommands.driveAtOrientation(
        () -> -driverJoystick.getY(),
        () -> -driverJoystick.getX(),
        () -> false,
        () -> Constants.Field.BLUE_REEF_ANGLE_ONE,
        Constants.Drive.MAX_SNAP_SPEED_PERCENTAGE
    )
);
// Buttons 2-6 snap to other reef angles
// Button 7-8 snap to station angles
// Button 9 snaps to processor angle
```

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

### Operator Controls for Fine Elevator Adjustment

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

Configures operator buttons for fine-tuning the elevator's vertical position. These commands incrementally adjust the elevator height up or down by a specified constant value.

```java
// Operator controls - Fine adjustment
operatorButtons.button(9).onTrue(
    ElevatorCommands.modifyHeight(Constants.Elevator.ELEVATOR_MODIFICATION_HEIGHT)
);  // +0.5"
operatorButtons.button(10).onTrue(
    ElevatorCommands.modifyHeight(-Constants.Elevator.ELEVATOR_MODIFICATION_HEIGHT)
);  // -0.5"
```

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

### Driver Utility Controls

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

Maps utility functions to specific driver joystick buttons. This includes enabling a slow mode for precise movement and a command to reset the robot's heading (gyroscope).

```java
// Driver utility controls
driverJoystick.button(2).whileTrue(DriveCommands.reduceSpeed());  // Slow mode
driverJoystick.button(11).onTrue(DriveCommands.resetGyro());      // Reset heading
```

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

### Hang Sequence Trigger and Manual Control

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

Sets up a composite trigger for initiating the hang sequence, requiring confirmation from both the operator and driver. It also includes a manual control for the elevator during hanging operations when a specific trigger is held.

```java
// Hang sequence - Requires both driver and operator confirmation
Trigger operatorHang = operatorButtons.axisLessThan(1, -0.5)
    .or(operatorButtons.povDown());
Trigger hangTrigger = operatorHang.and(driverJoystick.button(4));

hangTrigger.onTrue(
    Commands.sequence(
        FunnelCommands.drop(),
        ElevatorCommands.setHeight(ElevatorHeight.Hang)
    )
);

// Manual hang control (while held)
Trigger manualHangUp = operatorButtons.axisGreaterThan(1, 0.5)
    .or(operatorButtons.povUp());
manualHangUp.whileTrue(ElevatorCommands.hangExecute());
```

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

### Operator Controls for Manipulator Operations

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

Maps operator buttons to control the robot's manipulator. This includes initiating an intake sequence, stopping manipulator motors, and executing an output (eject) command followed by stowing.

```java
// Operator controls - Manipulator operations
operatorButtons.button(6).onTrue(CompositeCommands.fancyIntake());  // Smart intake
operatorButtons.button(7).onTrue(ManipulatorCommands.stop());       // Stop motors
operatorButtons.button(8).onTrue(CompositeCommands.output());       // Eject and stow
```

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

### Control Elevator Mechanism Position - Java

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

Manages the vertical extension of the elevator mechanism using PID control for precise positioning. It supports setting predefined height levels, custom heights, incremental adjustments, and manual voltage control. The subsystem also provides feedback on its current extension and setpoint status.

```java
// Initialize elevator subsystem
Elevator elevator = Elevator.getInstance();

// Set elevator to predefined height level
elevator.setExtension(ElevatorHeight.Level3);

// Set elevator to custom height (in inches from minimum extension)
elevator.setExtension(45.0);

// Check if elevator has reached target position
if (elevator.atSetpoint()) {
    System.out.println("Elevator ready at position: " + elevator.getExtension());
}

// Modify current setpoint by incremental amount
elevator.modifySetpoint(2.5);  // Add 2.5 inches to current target

// Manual voltage control (bypasses PID)
elevator.setVolts(6.0);  // Apply 6V to motors

// Get current elevator extension
double currentHeight = elevator.getExtension();
System.out.println("Current height: " + currentHeight + " inches");

// Available preset heights:
// ElevatorHeight.Stow    - 18.0 inches (retracted)
// ElevatorHeight.Level1  - 27.0 inches
// ElevatorHeight.Level2  - 33.4 inches
// ElevatorHeight.Level3  - 48.5 inches
// ElevatorHeight.Level4  - 75.5 inches
// ElevatorHeight.Hang    - 40.0 inches (for climbing)
```

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

### Operator Controls for Elevator Positioning

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

Assigns operator button inputs to control the elevator's height. Each button triggers a command to set the elevator to a specific predefined level or a stow position.

```java
// Operator controls - Elevator positioning
operatorButtons.button(1).onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level4));
operatorButtons.button(2).onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level3));
operatorButtons.button(3).onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level2));
operatorButtons.button(4).onTrue(ElevatorCommands.setHeight(ElevatorHeight.Level1));
operatorButtons.button(5).onTrue(ElevatorCommands.setHeight(ElevatorHeight.Stow));
```

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

### Control Swerve Drive Robot Movement and Odometry - Java

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

Controls the swerve drive robot's movement using field-relative speeds and manages odometry. It allows for setting robot velocity, retrieving current pose, resetting odometry, and accessing individual swerve module states. This functionality is crucial for navigation and autonomous operations.

```java
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.Units;

// Initialize the drive subsystem (singleton pattern)
Drive drive = Drive.getInstance();

// Run robot at specific velocity (field-relative)
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
    2.0,  // vx in m/s
    1.0,  // vy in m/s
    0.5,  // omega in rad/s
    drive.getRotation()  // current robot rotation
);
drive.runVelocity(speeds);

// Get current robot pose on field
Pose2d currentPose = drive.getPose();
System.out.println("X: " + currentPose.getX() + " Y: " + currentPose.getY());

// Reset odometry to known position
drive.setPose(new Pose2d(
    Units.inchesToMeters(297.5),
    Units.inchesToMeters(158.5),
    Rotation2d.fromDegrees(0)
));

// Get module states for all swerve modules
SwerveModuleState[] states = drive.getModuleStates();
for (int i = 0; i < states.length; i++) {
    System.out.println("Module " + i + ": " +
        states[i].speedMetersPerSecond + " m/s @ " +
        states[i].angle.getDegrees() + " degrees");
}
```
Tokens:
Raw