From 76b53c95a7283616a15009c186b973b5d170f716 Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Thu, 9 Jan 2025 19:40:37 -0700
Subject: [PATCH] e

---
 src/main/java/frc4388/robot/Constants.java    |  6 ++---
 .../java/frc4388/robot/RobotContainer.java    |  1 +
 .../frc4388/robot/subsystems/SwerveDrive.java |  9 +++++++-
 .../java/frc4388/robot/subsystems/Vision.java | 22 ++++++++++++++++---
 4 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 61cbf07..bd8ee92 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -276,11 +276,11 @@ public final class Constants {
       }
     
     public static final class VisionConstants {
-        public static final String CAMERA_NAME = "photonvision";
+        public static final String CAMERA_NAME = "Camera_Module_v1";
 
         public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d());
 
-        public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+        public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
 
         // The standard deviations of our vision estimated poses, which affect correction rate
         // (Fake values. Experiment and determine estimation noise on an actual robot.)
@@ -305,4 +305,4 @@ public final class Constants {
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 3be1700..bd0889d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -57,6 +57,7 @@ public class RobotContainer {
     public final Vision m_vision = new Vision(m_robotMap.camera);
 
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
+    // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
 
     /* Controllers */
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 09b3bb5..4894fb4 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -49,6 +49,7 @@ public class SwerveDrive extends Subsystem {
 
   /** Creates a new SwerveDrive. */
   public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
+    // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
     super();
 
     this.swerveDriveTrain = swerveDriveTrain;
@@ -172,7 +173,13 @@ public class SwerveDrive extends Subsystem {
     SmartDashboard.putNumber("Gyro", getGyroAngle());
     SmartDashboard.putNumber("RotTartget", rotTarget);
 
-    vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(Vision.getTime()));
+    double time = Vision.getTime();
+
+    vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
+
+    if(vision.isTag()){
+      swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
+    }
 
     // if(e.isPresent())
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java
index 899dba0..6848989 100644
--- a/src/main/java/frc4388/robot/subsystems/Vision.java
+++ b/src/main/java/frc4388/robot/subsystems/Vision.java
@@ -56,9 +56,15 @@ public class Vision extends Subsystem {
     public void periodic() {
         var result = camera.getLatestResult();
         isTag = result.hasTargets();
+        
+        field.setRobotPose(lastPhysOdomPose);
+
+        // sbCamConnected.setBoolean(camera);
+
+        // System.out.println(isTag);
 
         if(!isTag){
-            field.setRobotPose(lastPhysOdomPose);
+            sbTag.setBoolean(isTag);
             return;
         }
 
@@ -66,12 +72,11 @@ public class Vision extends Subsystem {
 
         // In case the pose estimator fails to estimate the pose, fallback to physical odometry.
         if(EstimatedRobotPose.isEmpty()){
+            sbTag.setBoolean(isTag);
             field.setRobotPose(lastPhysOdomPose);
             isTag = false;
             return;
         }
-
-        field.setRobotPose(lastVisionPose);
     }
 
 
@@ -196,6 +201,11 @@ public class Vision extends Subsystem {
         return Utils.getCurrentTimeSeconds();
     }
 
+    public boolean isTag(){
+        return isTag;
+    }
+
+
 
 
 
@@ -219,6 +229,11 @@ public class Vision extends Subsystem {
   .add("Tag Detected", false)
   .withWidget(BuiltInWidgets.kBooleanBox)
   .getEntry();
+  
+  GenericEntry sbCamConnected = subsystemLayout
+  .add("Camera Connnected", false)
+  .withWidget(BuiltInWidgets.kBooleanBox)
+  .getEntry();
 
 //   GenericEntry sbShiftState = subsystemLayout
 //   .add("Shift State", 0)
@@ -229,6 +244,7 @@ public class Vision extends Subsystem {
     @Override
     public void queryStatus() {
         sbTag.setBoolean(isTag);
+        sbCamConnected.setBoolean(camera.isConnected());
         field.setRobotPose(getPose2d());
     }