brought back vision/note align to how they were when they worked

Team334 · Mar 30, 2024 · 9bd7ad9 · 9bd7ad9
1 parent 290c6d3
commit 9bd7ad9
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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) {