e

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;
 
     }
-}
+}

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 */

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())
   }

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());
     }