From 9bd7ad9384a4ba8abfdafe1a1322200ae85ddc65 Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Fri, 29 Mar 2024 20:37:16 -0400
Subject: [PATCH] brought back vision/note align to how they were when they
 worked

---
 src/main/java/frc/robot/Constants.java            |  1 +
 src/main/java/frc/robot/RobotContainer.java       |  3 ++-
 .../java/frc/robot/commands/auto/NoteAlign.java   |  2 +-
 .../robot/subsystems/SwerveDriveSubsystem.java    | 15 ++++++++-------
 4 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 226fd83d..57e117cf 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -184,6 +184,7 @@ public static class FieldConstants {
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
 
     public static final double TAG_DISTANCE_THRESHOLD = 3.5;
+    public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 2;
 
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cabe4ba4..44527f20 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
+import frc.robot.Constants.Ports;
 import frc.robot.Constants.Presets;
 import frc.robot.commands.auto.AutoAim;
 import frc.robot.commands.auto.AutonShoot;
@@ -53,7 +54,7 @@ public class RobotContainer {
   private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem);
   private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem();
   private final IntakeSubsystem _intakeSubsystem = new IntakeSubsystem();
-  private final LEDSubsystem _ledSubsystem = new LEDSubsystem(0, 100);
+  private final LEDSubsystem _ledSubsystem = new LEDSubsystem(Ports.LEDS, 35);
   private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem();
 
   // controllers (for driver and operator)
diff --git a/src/main/java/frc/robot/commands/auto/NoteAlign.java b/src/main/java/frc/robot/commands/auto/NoteAlign.java
index 471ca1fb..96ad24fb 100644
--- a/src/main/java/frc/robot/commands/auto/NoteAlign.java
+++ b/src/main/java/frc/robot/commands/auto/NoteAlign.java
@@ -82,6 +82,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return _headingController.atSetpoint();
+    return false;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index 6e7f1538..b4e75074 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -215,15 +215,16 @@ private void updateBotpose() {
       if (tagCount >= 2) {
         xyStds = 0.55;
       }
-
-      // good distance
-      else if (tagDistance > 0.8 && poseDifference >= 0.5) {
+      
+      // one tag, closer distance, estimated pose is inaccurate
+      else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 3) {
         xyStds = 0.65;
       }
 
-      // else if (tagDistance > 0.3 && poseDifference >= 0.3) {
-
-      // }
+      // one tag, closer distance, estimated pose is accurate
+      else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 0.5) {
+        xyStds = 0.85;
+      }
 
       else {
         return;
@@ -288,7 +289,7 @@ public double getDriveCoeff() {
   public void driveChassis(ChassisSpeeds chassisSpeeds) {
     chassisSpeeds.vxMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vxMetersPerSecond, .01);
     chassisSpeeds.vyMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vyMetersPerSecond, .01);
-    chassisSpeeds.omegaRadiansPerSecond = MathUtil.applyDeadband(chassisSpeeds.omegaRadiansPerSecond, Math.PI / 20);
+    chassisSpeeds.omegaRadiansPerSecond = MathUtil.applyDeadband(chassisSpeeds.omegaRadiansPerSecond, Math.PI / 30);
 
     // IMPORTANT: X-axis and Y-axis are flipped (based on wpilib coord system)
     if (fieldOriented) {