added swerve heading to autoaim2

src/main/java/frc/robot/commands/shooter/AutoAim2.java

@@ -8,6 +8,7 @@
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.elevator.SetElevator;
+import frc.robot.commands.swerve.SetHeading;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
@@ -49,7 +50,7 @@ private AutoAim2(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      // swerve heading command here
+      new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
       new SetShooter(shooter, shooterAngle, runOnce),
       new SetElevator(elevator, elevatorHeight, runOnce)
       // led command here

src/main/java/frc/robot/commands/shooter/SpinShooter.java

@@ -2,7 +2,6 @@
 package frc.robot.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.Speeds;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterState;
 
@@ -14,12 +13,12 @@
 public class SpinShooter extends Command {
   private final ShooterSubsystem _shooter;
   private final ShooterState _state;
-  private final boolean _instant;
+  private final boolean _runOnce;
 
-  public SpinShooter(ShooterSubsystem shooter, ShooterState state, boolean instant) {
+  public SpinShooter(ShooterSubsystem shooter, ShooterState state, boolean runOnce) {
     _shooter = shooter;
     _state = state;
-    _instant = instant;
+    _runOnce = runOnce;
 
     // NO SHOOTER SUBSYSTEM REQUIREMENT TO NOT MESS WITH SHOOTER ANGLING COMMANDS
   }
@@ -48,6 +47,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return _instant;
+    return _runOnce;
   }
 }

src/main/java/frc/robot/commands/swerve/SetHeading.java

@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.swerve;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.Constants.Speeds;
+import frc.robot.subsystems.SwerveDriveSubsystem;
+
+public class SetHeading extends Command {
+  private SwerveDriveSubsystem _swerve;
+  private DoubleSupplier _heading;
+  private boolean _runOnce;
+
+  private DoubleSupplier _xSpeed;
+  private DoubleSupplier _ySpeed;
+
+  private PIDController _headingController = new PIDController(Constants.PID.SWERVE_HEADING_KP, 0, Constants.PID.SWERVE_HEADING_KD);
+
+  /**
+   * Creates a new SetHeading.
+   *
+   * @param heading The double supplier that returns the desired heading of the chassis in degrees.
+   * @param xSpeed X drive joystick speed.
+   * @param ySpeed Y drive joystick speed.
+   * @param runOnce Used to run this command for a changing setpoint.
+   */
+  public SetHeading(
+    SwerveDriveSubsystem swerve, 
+    DoubleSupplier xSpeed,
+    DoubleSupplier ySpeed, 
+    DoubleSupplier heading, 
+    boolean runOnce
+  ) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    _swerve = swerve;
+    _heading = heading;
+    _runOnce = runOnce;
+
+    _xSpeed = xSpeed;
+    _ySpeed = ySpeed;
+
+    addRequirements(_swerve);
+  }
+
+  /** Creates a new SetHeading that runs once. */
+  public SetHeading(
+    SwerveDriveSubsystem swerve,
+    DoubleSupplier xSpeed,
+    DoubleSupplier ySpeed,
+    DoubleSupplier heading
+  ) {
+    this(swerve, xSpeed, ySpeed, heading, true);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double rotationVelocity = MathUtil.clamp(
+      -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
+      Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
+      _headingController.calculate(_heading.getAsDouble())
+    );
+
+    _swerve.driveChassis(new ChassisSpeeds(
+      _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * _swerve.getDriveCoeff(),
+      _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * _swerve.getDriveCoeff(),
+      rotationVelocity
+    ));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    _swerve.driveChassis(new ChassisSpeeds());
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return _runOnce && _headingController.atSetpoint();
+  }
+}