Add choreo

[GrumpyBud]

This pull request introduces Choreo-based autonomous trajectory support to the robot codebase. It adds a new Autos class for managing autonomous routines using Choreo, integrates Choreo auto routines into the robot container and auto chooser, and updates the Drive subsystem to support trajectory following using PID control. Additionally, new Choreo trajectory and configuration files are included for autonomous routines.

Choreo Autonomous Integration:

• Added a new Autos class to encapsulate Choreo-based autonomous routines, including a "Depot Auto" routine that uses Choreo trajectory files. (src/main/java/frc/robot/Autos.java)
• Integrated the Autos class into RobotContainer, initializing it and adding the "Depot Auto (Choreo)" option to the dashboard auto chooser. (src/main/java/frc/robot/RobotContainer.java) [1] [2]

Drive Subsystem Enhancements:

• Added PID controllers for X, Y, and heading control to the Drive subsystem for precise trajectory following. (src/main/java/frc/robot/subsystems/drive/Drive.java) [1] [2]
• Implemented a followTrajectory method in Drive that consumes SwerveSample objects and uses the PID controllers to generate chassis speeds for trajectory tracking. (src/main/java/frc/robot/subsystems/drive/Drive.java)
• Enabled continuous input for the heading PID controller to handle angle wrapping. (src/main/java/frc/robot/subsystems/drive/Drive.java)

Choreo Trajectory and Configuration Files:

• Added a Choreo trajectory file (OutputAuto.traj) and a configuration file (depotAuto.chor) for the new autonomous routine. [1] [2]

[Copilot]

Pull request overview

This pull request introduces Choreo-based autonomous trajectory support to enable advanced path following capabilities for the robot. The changes integrate the ChoreoLib library and add infrastructure for defining and executing Choreo trajectories.

Changes:

• Added ChoreoLib vendor dependency (version 2026.0.1) for trajectory generation and following
• Created new Autos class to manage Choreo autonomous routines and integrated it with the robot container
• Enhanced Drive subsystem with PID-based trajectory following capability using feedforward + feedback control

Reviewed changes

Copilot reviewed 7 out of 7 changed files in this pull request and generated 7 comments.

Show a summary per file

File	Description
vendordeps/ChoreoLib2026.json	Adds ChoreoLib vendor dependency for 2026 season
src/main/java/frc/robot/subsystems/drive/Drive.java	Adds PID controllers and followTrajectory method for Choreo integration
src/main/java/frc/robot/RobotContainer.java	Integrates Autos class and adds Choreo autonomous option to dashboard
src/main/java/frc/robot/Autos.java	New class that manages Choreo autonomous routines with AutoFactory
depotAuto.chor	Choreo configuration file (incorrectly placed in root directory)
OutputAuto.traj	Trajectory file (incorrectly placed in root directory, unclear naming)
DepotAuto.traj	Trajectory file (incorrectly placed in root directory)

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

The trajectory files (OutputAuto.traj, DepotAuto.traj) and configuration file (depotAuto.chor) are placed in the project root directory, but they should be located in src/main/deploy/choreo/ for Choreo to load them properly at runtime. The AutoFactory will look for trajectory files in the deploy directory on the robot. Move these files to the correct location to ensure they can be loaded during autonomous execution.

[Copilot]

The trajectory file (DepotAuto.traj) is placed in the project root directory, but it should be located in src/main/deploy/choreo/ for Choreo to load it properly at runtime. Move this file to the correct location to ensure it can be loaded during autonomous execution.

[Copilot]

The configuration file (depotAuto.chor) is placed in the project root directory, but it should be located in src/main/deploy/choreo/ for Choreo to load it properly at runtime. Move this file to the correct location to ensure it can be loaded during autonomous execution.

[Copilot]

There is a naming inconsistency between the trajectory files. The code references "depotAuto" in line 47, but there are two trajectory files: "OutputAuto.traj" and "DepotAuto.traj". The file naming should match the trajectory name used in the code. Either rename "OutputAuto.traj" to match the intended trajectory name or update the code to reference the correct trajectory file name.

Suggested change

	AutoTrajectory depotTraj = routine.trajectory("depotAuto");
	AutoTrajectory depotTraj = routine.trajectory("DepotAuto");

[Copilot]

The fields flywheel, hood, and choreographer are stored but never used in this class. If these subsystems are not needed for the Choreo autonomous routines, they should be removed from the constructor parameters and field declarations. If they will be used in future autonomous routines, consider adding a comment explaining their intended use.

Suggested change

	import frc.robot.subsystems.Choreographer;
	import frc.robot.subsystems.drive.Drive;
	import frc.robot.subsystems.shooter.flywheel.Flywheel;
	import frc.robot.subsystems.shooter.hood.Hood;
	public class Autos {
	private final Drive drive;
	private final Flywheel flywheel;
	private final Hood hood;
	private final Choreographer choreographer;
	private final AutoFactory autoFactory;
	private final AutoChooser autoChooser;
	public Autos(Drive drive, Flywheel flywheel, Hood hood, Choreographer choreographer) {
	this.drive = drive;
	this.flywheel = flywheel;
	this.hood = hood;
	this.choreographer = choreographer;
	import frc.robot.subsystems.drive.Drive;
	public class Autos {
	private final Drive drive;
	private final AutoFactory autoFactory;
	private final AutoChooser autoChooser;
	public Autos(Drive drive) {
	this.drive = drive;

[Copilot]

The autoChooser field is initialized and populated but the getAutoChooser() method is never called in the PR. If this is intended for future use or integration with a dashboard, consider documenting its purpose. If it's not needed, the field and method should be removed to avoid confusion.

[Copilot]

The PID controller gains (P=10.0 for translation, P=7.5 for heading) appear to be placeholder values and may need tuning for the specific robot. Consider documenting these as initial values that require tuning, or extracting them to constants for easier adjustment during testing.

[Copilot]

Pull request overview

Copilot reviewed 10 out of 11 changed files in this pull request and generated 8 comments.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

The WHEEL_COF (coefficient of friction) constant in the Drive class (1.916) matches the depotAuto.chor configuration (1.916), but differs from the value in the trajectory files (1.2). This is correct since the .chor file is the source configuration, and the trajectories should be regenerated to match. Once the trajectories are regenerated with the correct configuration, this constant will be consistent throughout.

[Copilot]

The depotAuto routine uses Commands.sequence() to first reset odometry and then run the trajectory. However, this creates a sequential command where resetOdometry() completes before the trajectory starts. If resetOdometry() is instantaneous (returns immediately), this is fine. But if you want both to start simultaneously, consider using Commands.parallel(). Also verify that routine.active().onTrue() is the correct trigger - this should activate when the autonomous routine becomes active, but ensure this matches your intended autonomous workflow.

[Copilot]

The trajectory configuration embedded in the .traj files has inconsistent physical parameters compared to the .chor configuration file. Specifically:

In depotAuto.chor:

• mass: 64 lb = 29.03 kg (line 31-32)
• inertia: 2.11 kg*m^2 (line 34-36)
• cof: 1.916 (line 54-56)
• gearing: 6.746031746 (line 38-40)
• frontLeft position: 0.263525 m (10.375 in)

But in both OutputAuto.traj and DepotAuto.traj trajectory configs:

• mass: 56.69904625 kg (line 52 in DepotAuto.traj, line 39 in OutputAuto.traj)
• inertia: 9.0 kg*m^2 (line 53, line 40)
• cof: 1.2 (line 58, line 45)
• gearing: 6.75 (line 54, line 41)
• frontLeft position: 0.3302 m (line 45, line 32)

These trajectories were likely generated with different robot configurations than specified in depotAuto.chor. The embedded trajectory physics won't match the actual robot, which could cause tracking errors or unexpected behavior. Regenerate the trajectories using the correct configuration from depotAuto.chor, or update depotAuto.chor to match the configuration used during generation.

[Copilot]

The OutputAuto.traj file is not referenced anywhere in the codebase. This appears to be an unused trajectory file that should either be removed or integrated into the autonomous routines. If this is a work-in-progress trajectory, consider moving it to a different location or adding a comment explaining its purpose.

[Copilot]

The followTrajectory method constructs ChassisSpeeds from Choreo trajectory samples, which are field-relative (world frame). However, the runVelocity method expects robot-relative speeds. The SwerveSample values (vx, vy, omega) from Choreo are in field coordinates, but you're passing them directly to runVelocity without converting from field-relative to robot-relative coordinates.

You need to convert the speeds using ChassisSpeeds.fromFieldRelativeSpeeds() before passing to runVelocity, or ensure runVelocity handles field-relative speeds. Check the existing DriveCommands or other trajectory following code to see how field vs robot relative speeds are handled in this codebase.

Suggested change

	new ChassisSpeeds(
	sample.vx + xController.calculate(pose.getX(), sample.x),
	sample.vy + yController.calculate(pose.getY(), sample.y),
	sample.omega
	+ headingController.calculate(pose.getRotation().getRadians(), sample.heading));
	ChassisSpeeds.fromFieldRelativeSpeeds(
	sample.vx + xController.calculate(pose.getX(), sample.x),
	sample.vy + yController.calculate(pose.getY(), sample.y),
	sample.omega
	+ headingController.calculate(pose.getRotation().getRadians(), sample.heading),
	pose.getRotation());

[Copilot]

The followTrajectory method lacks documentation. Add a Javadoc comment explaining:

• What the method does (follows a Choreo trajectory sample)
• The parameter (SwerveSample sample from Choreo trajectory)
• How it works (uses PID controllers for position tracking with feedforward from trajectory)
• Any important notes about coordinate frames or usage

This is especially important for a new public API that will be called by autonomous routines.

Suggested change

	/**
	* Follow a single Choreo swerve trajectory sample using closed-loop position control.
	*
	* <p>This method takes a {@link SwerveSample} generated by Choreo, which contains the desired
	* field-relative pose (x, y, heading) and chassis velocities (vx, vy, omega) for the current
	* time step. It computes corrections with the internal PID controllers for X position, Y
	* position, and heading based on the current estimated robot pose from {@link #getPose()}, then
	* adds those corrections as feedback terms on top of the feedforward velocities provided by the
	* trajectory sample before commanding the drive.
	*
	* <p><strong>Coordinate frames and usage:</strong>
	*
	* <ul>
	* <li>The sample pose and velocities are expected to be in field coordinates, with positions
	* in meters and heading in radians.
	* <li>The internal pose estimator must use the same field coordinate frame and origin as the
	* Choreo trajectory for accurate tracking.
	* <li>This method should be called periodically (e.g., from an autonomous routine) with
	* successive samples along the trajectory.
	* </ul>
	*
	* @param sample the current Choreo {@link SwerveSample} specifying desired field-relative pose
	* and chassis velocities to track
	*/

[Copilot]

The trajectory configuration embedded in this file has inconsistent physical parameters compared to depotAuto.chor. Specifically:

In depotAuto.chor:

• mass: 64 lb = 29.03 kg
• inertia: 2.11 kg*m^2
• cof: 1.916
• gearing: 6.746031746
• frontLeft position: 0.263525 m (10.375 in)

But in this trajectory config:

• mass: 56.69904625 kg
• inertia: 9.0 kg*m^2
• cof: 1.2
• gearing: 6.75
• frontLeft position: 0.3302 m

This trajectory was likely generated with different robot configurations than specified in depotAuto.chor. The embedded trajectory physics won't match the actual robot, which could cause tracking errors or unexpected behavior. Regenerate the trajectory using the correct configuration from depotAuto.chor, or update depotAuto.chor to match the configuration used during generation.

Suggested change

	"x":0.3302,
	"y":0.3302
	},
	"backLeft":{
	"x":-0.3302,
	"y":0.3302
	},
	"mass":56.69904625,
	"inertia":9.0,
	"gearing":6.75,
	"radius":0.0508,
	"vmax":628.3185307179587,
	"tmax":0.75,
	"cof":1.2,
	"x":0.263525,
	"y":0.263525
	},
	"backLeft":{
	"x":-0.263525,
	"y":0.263525
	},
	"mass":29.03,
	"inertia":2.11,
	"gearing":6.746031746,
	"radius":0.0508,
	"vmax":628.3185307179587,
	"tmax":0.75,
	"cof":1.916,

[Copilot]

The PID controller gains (Kp=10.0 for translation, Kp=7.5 for rotation) appear to be quite high and may cause oscillation or instability during trajectory following. These values should be tuned empirically on the actual robot. Consider starting with lower gains (e.g., Kp=5.0 for translation, Kp=5.0 for rotation) and tuning up from there. Also consider adding derivative or integral terms if needed for better trajectory tracking.

Suggested change

	private final PIDController xController = new PIDController(10.0, 0.0, 0.0);
	private final PIDController yController = new PIDController(10.0, 0.0, 0.0);
	private final PIDController headingController = new PIDController(7.5, 0.0, 0.0);
	private final PIDController xController = new PIDController(5.0, 0.0, 0.0);
	private final PIDController yController = new PIDController(5.0, 0.0, 0.0);
	private final PIDController headingController = new PIDController(5.0, 0.0, 0.0);