Auto Align Fixes and Logging

src/main/java/frc/robot/vision/VisionConfig.java

@@ -99,17 +99,15 @@ public class VisionConfig {
 
 
     public static class AlignmentConfig {
-        public static final double THETA_kP = 3.0;
-        public static final double THETA_kI = 0.0;
-        public static final double THETA_kD = 0.0;
-
-        public static final double X_kP = 5.0;
-        public static final double X_kI = 0.0;
-        public static final double X_kD = 0.0;
+
+        //Starting with same PID values as Auton
+        public static final double XY_kP = 10.0;
+        public static final double XY_kI = 0.0;
+        public static final double XY_kD = 0.0;
 
-        public static final double Y_kP = 5.0;
-        public static final double Y_kI = 0.0;
-        public static final double Y_kD = 0.0;
+        public static final double THETA_kP = 6.5;
+        public static final double THETA_kI = 0.0;
+        public static final double THETA_kD = 0.05;    
 
         public static final Distance DISTANCE_TOLERANCE = Inches.of(0.5);
         public static final Distance LATERAL_TOLERANCE = Inches.of(1.0);

src/main/java/frc/robot/vision/commands/AutoAlign.java

@@ -16,8 +16,8 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.RobotContainer;
 import frc.robot.drivetrain.CommandSwerveDrivetrain;
 import frc.robot.vision.PoseEstimatorSubsystem;
 
@@ -64,36 +64,33 @@ public class AutoAlign extends Command {
    */
   public AutoAlign(Pose2d targetPose) {
     this(drivetrainSubsystem, poseProvider);
-    setGoal(targetPose);
+    this.goalPose2d = targetPose;
+    // setGoal(targetPose);
   }
 
   /**
    * Constructs a DriveToPoseCommand with specific motion profile constraints
    * 
    * @param drivetrainSubsystem drivetrain subsystem
    * @param poseProvider provider to call to get the robot pose
-   * @param translationConstraints translation motion profile constraints
-   * @param omegaConstraints rotation motion profile constraints
    */
   public AutoAlign(
       CommandSwerveDrivetrain drivetrainSubsystem,
-      Supplier<Pose2d> goalPose) {
+      Supplier<Pose2d> poseProvider) {
 
     // this.drivetrainSubsystem = CommandSwerveDrivetrain.getInstance();
     // this.poseProvider = () -> PoseEstimatorSubsystem.getInstance().getCurrentPose();
 
-    xController = new PIDController(X_kP, X_kI, X_kD);
+    xController = new PIDController(XY_kP, XY_kI, XY_kD);
     xController.setTolerance(TRANSLATION_TOLERANCE.in(Meters));
 
-    yController = new PIDController(Y_kP, Y_kI, Y_kD);
+    yController = new PIDController(XY_kP, XY_kI, XY_kD);
     yController.setTolerance(TRANSLATION_TOLERANCE.in(Meters));
 
     thetaController = new PIDController(THETA_kP, THETA_kI, THETA_kD);
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
     thetaController.setTolerance(THETA_TOLERANCE.in(Radians));
 
-    this.goalPose2d = goalPose.get();
-
     addRequirements(drivetrainSubsystem);
   }
 
@@ -153,6 +150,24 @@ public void execute() {
     drivetrainSubsystem.setControl(applyFieldSpeeds.withSpeeds(speeds));
     // drivetrainSubsystem.setControl(
     //     fieldCentricSwerveRequest.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(omegaSpeed));
+    SmartDashboard.putNumber("Goal Pose X", this.goalPose2d.getX());
+    SmartDashboard.putNumber("Goal Pose Y", this.goalPose2d.getY());
+    SmartDashboard.putNumber("Goal Pose Theta", this.goalPose2d.getRotation().getRadians());
+    SmartDashboard.putNumber("Current Robot Pose X", robotPose.getX());
+    SmartDashboard.putNumber("Current Robot Pose Y", robotPose.getY());
+    SmartDashboard.putNumber("Current Robot Pose Theta", robotPose.getRotation().getRadians());
+    SmartDashboard.putNumber("X Setpoint", xController.getSetpoint());
+    SmartDashboard.putNumber("Y Setpoint", yController.getSetpoint());
+    SmartDashboard.putNumber("Theta Setpoint", thetaController.getSetpoint());
+    SmartDashboard.putNumber("X Output", xController.calculate(robotPose.getX()));
+    SmartDashboard.putNumber("Y Output", yController.calculate(robotPose.getY()));
+    SmartDashboard.putNumber("Theta Output", thetaController.calculate(robotPose.getRotation().getRadians()));
+    SmartDashboard.putNumber("X Error", xController.getError());
+    SmartDashboard.putNumber("Y Error", yController.getError());
+    SmartDashboard.putNumber("Theta Error", thetaController.getError());
+    SmartDashboard.putBoolean("X at Setpoint", xController.atSetpoint());
+    SmartDashboard.putBoolean("Y at Setpoint", yController.atSetpoint());
+    SmartDashboard.putBoolean("Theta at Setpoint", thetaController.atSetpoint());
   }
 
   @Override

src/main/java/frc/robot/vision/commands/LineupCommand.java

@@ -50,15 +50,15 @@ public class LineupCommand extends Command {
 
 
     private static final ProfiledPIDController xDistanceController = new ProfiledPIDController(
-                                        VisionConfig.AlignmentConfig.X_kP,
-                                        VisionConfig.AlignmentConfig.X_kI,
-                                        VisionConfig.AlignmentConfig.X_kD,
+                                        VisionConfig.AlignmentConfig.XY_kP,
+                                        VisionConfig.AlignmentConfig.XY_kI,
+                                        VisionConfig.AlignmentConfig.XY_kD,
                                         TRANSLATION_CONSTRAINTS);
 
     private static final ProfiledPIDController yDistanceController = new ProfiledPIDController(
-                                        VisionConfig.AlignmentConfig.Y_kP,
-                                        VisionConfig.AlignmentConfig.Y_kI,
-                                        VisionConfig.AlignmentConfig.Y_kD,
+                                        VisionConfig.AlignmentConfig.XY_kP,
+                                        VisionConfig.AlignmentConfig.XY_kI,
+                                        VisionConfig.AlignmentConfig.XY_kD,
                                         TRANSLATION_CONSTRAINTS);
 
     private static final ProfiledPIDController thetaController = new ProfiledPIDController(