moved heading pid controller for swerve auto aim into the swerve subs…

…ystem, so that auto led status can be added later

src/main/java/frc/robot/commands/auto/AutoAim.java

@@ -56,7 +56,7 @@ private AutoAim(
       new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
       new SetShooter(shooter, shooterAngle, runOnce),
       new SetElevator(elevator, elevatorHeight, runOnce)
-      // led command here
+      // some led command here
     );
   }
 

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

@@ -6,12 +6,8 @@
 
 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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.PID;
 import frc.robot.Constants.Speeds;
 import frc.robot.subsystems.SwerveDriveSubsystem;
 
@@ -23,8 +19,6 @@ public class SetHeading extends Command {
   private DoubleSupplier _xSpeed;
   private DoubleSupplier _ySpeed;
 
-  private PIDController _headingController = new PIDController(PID.SWERVE_HEADING_KP, 0, PID.SWERVE_HEADING_KD);
-
   /**
    * Creates a new SetHeading.
    *
@@ -48,9 +42,6 @@ public SetHeading(
     _xSpeed = xSpeed;
     _ySpeed = ySpeed;
 
-    _headingController.setTolerance(3);
-    _headingController.enableContinuousInput(-180, 180);
-
     addRequirements(_swerve);
   }
 
@@ -71,19 +62,11 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    SmartDashboard.putData(_headingController);
-
-    double rotationVelocity = MathUtil.clamp(
-      _headingController.calculate(_swerve.getHeading().getDegrees(), _heading.getAsDouble()),
-      -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
-      Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED
-    );
-
-    _swerve.driveChassis(new ChassisSpeeds(
+    _swerve.setHeading(
       _xSpeed.getAsDouble() * Speeds.SWERVE_DRIVE_MAX_SPEED * _swerve.getDriveCoeff(),
       _ySpeed.getAsDouble() * Speeds.SWERVE_DRIVE_MAX_SPEED * _swerve.getDriveCoeff(),
-      rotationVelocity
-    ));
+      _heading.getAsDouble()
+    );
   }
 
   // Called once the command ends or is interrupted.
@@ -95,6 +78,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return _runOnce && _headingController.atSetpoint();
+    return _runOnce && _swerve.atDesiredHeading();
   }
 }

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -10,6 +10,7 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -29,6 +30,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.PID;
+import frc.robot.Constants.Speeds;
 import frc.robot.utils.SwerveModule;
 import frc.robot.utils.UtilFuncs;
 
@@ -60,7 +63,8 @@ public class SwerveDriveSubsystem extends SubsystemBase {
 
   // private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
   //     BNO055.vector_type_t.VECTOR_EULER);
-
+  private PIDController _headingController = new PIDController(PID.SWERVE_HEADING_KP, 0, PID.SWERVE_HEADING_KD);
+
   private final AHRS _gyro = new AHRS();
 
   private VisionSubsystem _visionSubsystem;
@@ -113,9 +117,8 @@ public ChassisSpeeds getRobotRelativeSpeeds() {
   public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) {
     _visionSubsystem = visionSubsystem;
 
-    // resetPose(
-    //   new Pose2d(2.52, 5.25, Rotation2d.fromDegrees(180))
-    // ); // for testing
+    _headingController.setTolerance(3);
+    _headingController.enableContinuousInput(-180, 180);
 
     // setupOrchestra();
 
@@ -285,6 +288,35 @@ public void driveChassis(ChassisSpeeds chassisSpeeds) {
     setStates(moduleStates);
   }
 
+  /**
+   * Return whether the chassis is at the last desired set heading.
+   */
+  public boolean atDesiredHeading() {
+    return _headingController.atSetpoint();
+  }
+
+  /**
+   * Sets the chassis to a desired heading while driving.
+   * 
+   * @param xSpeed X robot speed.
+   * @param ySpeed Y robot speed.
+   * 
+   * @param heading The heading to set to.
+   */
+  public void setHeading(double xSpeed, double ySpeed, double heading) {
+    double rotationVelocity = MathUtil.clamp(
+      _headingController.calculate(getHeading().getDegrees(), heading),
+      -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
+      Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED
+    );
+
+    driveChassis(new ChassisSpeeds(
+      xSpeed,
+      ySpeed,
+      rotationVelocity
+    ));
+  }
+
   /**
    * Sets the state of each SwerveModule through an array.
    *