From a0e7b266e18b5a5185d75f45db59944bb68b497d Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Fri, 26 Jan 2024 14:56:37 -0500
Subject: [PATCH] debug for auto aim test

---
 .../frc/robot/commands/shooter/AutoAim.java   | 19 ++++++++++++++-----
 .../subsystems/SwerveDriveSubsystem.java      |  2 ++
 .../frc/robot/subsystems/VisionSubsystem.java |  2 +-
 3 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java
index d75b24f..b419cac 100644
--- a/src/main/java/frc/robot/commands/shooter/AutoAim.java
+++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java
@@ -62,24 +62,33 @@ public void execute() {
 
     double currentSwerveHeading = _swerve.getHeading().getDegrees();
 
-    if (_vision.isApriltagVisible()) {
-      desiredShooterAngle = _vision.anglesToSpeaker()[1]; // ty
-      desiredSwerveHeading = _vision.anglesToSpeaker()[0];
+    SmartDashboard.putBoolean("VISIBLE TAG", _vision.isApriltagVisible());
+
+    double[] visionAngles = _vision.anglesToSpeaker();
+
+    if (_vision.isApriltagVisible() && visionAngles != null) {
+      desiredShooterAngle = visionAngles[1];
+      desiredSwerveHeading = visionAngles[0];
+
+      SmartDashboard.putNumberArray("VISION ANGLES", visionAngles);
 
     } else {
       desiredShooterAngle = _swerve.anglesToSpeaker()[1];
       desiredSwerveHeading = _swerve.anglesToSpeaker()[0];
     }
 
-    desiredSwerveHeading += currentSwerveHeading;
+    desiredSwerveHeading -= currentSwerveHeading;
 
     _shooter.setAngle(desiredShooterAngle);
 
+    SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading);
+
     _swerve.driveChassis(
       new ChassisSpeeds(
         _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
         _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
-        _headingController.calculate(currentSwerveHeading, desiredSwerveHeading)
+        // _headingController.calculate(currentSwerveHeading, desiredSwerveHeading),
+        0
       )
     );
   }
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index d8f2004..4e7055e 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -156,6 +156,8 @@ public ChassisSpeeds getRobotRelativeSpeeds() {
   public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) {
     _visionSubsystem = visionSubsystem;
 
+    resetPose(_visionSubsystem.getBotpose().get());
+
     // setupOrchestra();
 
     // pathplannerlib setup
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
index 782776c..c004377 100644
--- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -100,7 +100,7 @@ public boolean isApriltagVisible() {
   public boolean isApriltagVisible(int ID) {
     if (!isApriltagVisible()) return false;
 
-    return _limelight.getTag(ID) != null;
+    return (_limelight.getTag(ID) != null);
   }
 
   /**