Choreo routines

[SnappleRamen]

This branch added Choreo autos that the robot can run in simulation.

[markpete]

/gemini review

[gemini-code-assist]

Code Review

This pull request introduces Choreo autonomous routines, which is a great addition. My review focuses on ensuring the new code is robust, maintainable, and free of bugs. I've found a critical compilation error in Robot.java due to an extra brace, along with a few high-severity issues related to shared state in PID controllers and incorrect use of units in the simulation code. There are also several medium-severity suggestions to improve code quality, such as removing unused code, fixing formatting, and improving encapsulation. Please review the detailed comments for suggestions on how to address these points.

[gemini-code-assist]

This method has a few issues:

1. Critical Syntax Error: There's an extra closing brace } on line 181 that will cause a compilation failure.
2. High Severity Logic Change: The if (subsystems.climbPivotSubsystem != null) block has been nested inside the outer if block. This is likely unintentional and changes the behavior of the autonomous initialization, as moveCompleteFalse() will now only be called if a drivebase is enabled and an auto is selected.
3. Readability: The code contains commented-out lines and excessive whitespace, which hurts readability.

I've provided a suggestion that fixes all these issues by cleaning up the code, removing the extra brace, and restoring the original logic for the conditional blocks.

  public void autonomousInit() {
    Shuffleboard.startRecording();
    if (SubsystemConstants.DRIVEBASE_ENABLED && AutoLogic.getSelectedAuto() != null) {
      new Routines().runRoutine().schedule();
    }

    if (subsystems.climbPivotSubsystem != null) {
      subsystems.climbPivotSubsystem.moveCompleteFalse();
    }
  }

[gemini-code-assist]

The Rotation3d constructor expects angles in radians, but it appears you are passing degrees for the roll component (-90). This will result in an incorrect rotation in the simulation. You should convert the degree values to radians using Math.toRadians(). The wristPos value is also likely in degrees and should be converted as well.

Suggested change

	m_posePublisher.set(new Pose3d(0.2, 0.0, targetZ, new Rotation3d(-90, wristPos,-0)));
	m_posePublisher.set(new Pose3d(0.2, 0.0, targetZ, new Rotation3d(Math.toRadians(-90), Math.toRadians(wristPos), 0)));

[gemini-code-assist]

The .withSpeeds(speeds) method is called twice here. This is redundant and likely a copy-paste error. The second call should be removed.

          this.setControl(new SwerveRequest.ApplyFieldSpeeds().withSpeeds(speeds));

[gemini-code-assist]

There are a couple of issues with these PID controllers:

1. pidY is incorrectly initialized with AutoAlign.AutoAlignCommand.pidX instead of pidY. This is a bug.
2. These PID controllers are being shared from AutoAlign.AutoAlignCommand where they are public static. This is a poor design practice as it creates shared mutable state between different components (AutoAlignCommand and CommandSwerveDrivetrain), which can lead to unexpected behavior. This class should have its own private, final PID controller instances for trajectory following. The corresponding fields in AutoAlign.AutoAlignCommand should be reverted to be non-static instance fields.

Suggested change

	public PIDController pidX = AutoAlign.AutoAlignCommand.pidX;
	public PIDController pidY = AutoAlign.AutoAlignCommand.pidX;
	public PIDController pidRotate = AutoAlign.AutoAlignCommand.pidRotate;
	public PIDController pidX = new PIDController(4, 0, 0);
	public PIDController pidY = new PIDController(4, 0, 0);
	public PIDController pidRotate = new PIDController(8, 0, 0);

[gemini-code-assist]

These PID controllers should not be public static. Making them static creates a shared, mutable state between all instances of AutoAlignCommand and other classes that use them (like CommandSwerveDrivetrain). This is a dangerous pattern that can lead to unpredictable behavior and hard-to-debug issues. They should be instance fields (e.g., private final) to ensure each command has its own state.

Suggested change

	public static PIDController pidX = new PIDController(4, 0, 0);
	public static PIDController pidY = new PIDController(4, 0, 0);
	public static PIDController pidRotate = new PIDController(8, 0, 0);
	private final PIDController pidX = new PIDController(4, 0, 0);
	private final PIDController pidY = new PIDController(4, 0, 0);
	private final PIDController pidRotate = new PIDController(8, 0, 0);

[gemini-code-assist]

There's a typo in this comment. "folloiwing" should be "following".

Suggested change

	/*AutoAlign PID used in constructor for folloiwing choreo trajectories */
	/*AutoAlign PID used in constructor for following choreo trajectories */

[gemini-code-assist]

The field name arm_Pivot does not follow standard Java naming conventions (camelCase). It should be renamed to armPivot for consistency.

Suggested change

	public DoubleTopic arm_Pivot = NetworkTableInstance.getDefault().getDoubleTopic("Arm Pivot");
	public DoubleTopic armPivot = NetworkTableInstance.getDefault().getDoubleTopic("Arm Pivot");

[gemini-code-assist]

The headingController field is declared but never initialized or used. It should be removed to avoid dead code.

[gemini-code-assist]

These are stray semicolons and should be removed.

[gemini-code-assist]

This import of frc.robot.Robot is unused and can be removed.