FRC Team 5414's code for the 2025 competition robot for Reefscape 🪸, Pearracuda 🐠. The code is written in Java and uses WPILib's Java command-based structure.
-
Field-Centric Swerve Drive
The robot's drivetrain is a standard swerve drivetrain with field-centric control using SDS MK4i modules with double Kraken X60 motors. The drivetrain uses encoders, a Pigeon 2 gyro, and odometry to control movement during the autonomous and teleoperated phases. The rotation of the drivetrain can be controlled either through speed or heading.
-
Elevator + Arm, End Effector/Intake Rollers, Climber Winch
The robot uses WPILib subsystems and enums to effectively create a state machine that controls each mechanism. The arm and elevator use Motion Magic to ensure precise, repeatable control, with both fixed setpoints and dynamic positions determined through inverse kinematics. The end effector uses voltage control to intake coral and algae, and position control to hold onto a game piece. The climber implements a state machine with positional PID control to grasp the deep cage and lift the robot.
-
Autonomous Path Following
The robot uses Team 3015's PathPlanner, a motion profile generator for FRC robots, to generate and follow autonomous trajectories. Autonomous routines are created using PathPlanner's built-in AutoBuilder and declaring NamedCommands with the PathPlanner application, and selected through sendable choosers in Elastic.
-
Reef and Coral Station Alignment/Limelight Vision
The robot uses Limelight's Vision Software to manage two mounted Limelight 4 cameras that provide real-time positional data of the robot based on field elements with Team 3636 General's vision backend code modified to support multiple cameras and MegaTag2. A single button press autonomously drives the robot to a pose a given offset from an April Tag using PID controllers for translational and rotational velocity, enabling swift alignment to the reef and coral stations.
-
Logging, Simulation, and Replay
The robot uses AdvantageKit made by 6328 Mechanical Advantage to log useful information about the robot for debugging purposes. This data can be analyzed in AdvantageScope or Pearascope, a custom spreadsheet generator that outputs key entries of interest to tell a story of what happened throughout a match. WPILib's simulation classes output data that can be used to verify code logic without a physical robot, and Team 5516's Maple Sim library models interactions between robots, field elements, and game pieces using a physics engine.