adjusting vision and odometry fusion

.OutlineViewer/outlineviewer.json

@@ -2,6 +2,9 @@
   "Clients": {
     "open": true
   },
+  "Connections": {
+    "open": true
+  },
   "NetworkTables Settings": {
     "mode": "Client (NT4)"
   },
@@ -14,6 +17,12 @@
     },
     "open": true
   },
+  "client@5": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
   "outlineviewer@2": {
     "Publishers": {
       "open": true

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -10,6 +10,18 @@
             "pathName": "Start to 30 Right"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "J Approach"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -21,6 +33,12 @@
           "data": {
             "pathName": "J Backup"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
         }
       ]
     }

src/main/deploy/pathplanner/autos/Two Coral Left.auto

@@ -13,13 +13,38 @@
         {
           "type": "named",
           "data": {
-            "name": "Shoot Coral L4"
+            "name": "Lift L4"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "30 Right to HP"
+            "pathName": "J Approach"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "30 Right to HP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
           }
         },
         {
@@ -34,6 +59,18 @@
             "pathName": "HP to 330 Left"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "K Approach"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/paths/150 Right to HP.path

@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.3072345890410957,
+        "x": 1.174795081967213,
-        "y": 7.135316780821918
+        "y": 7.321618852459016
       },
       "prevControl": {
-        "x": 2.1636986301369863,
+        "x": 2.031259123063103,
-        "y": 6.188698630136986
+        "y": 6.375000701774084
       },
       "nextControl": null,
       "isLocked": false,
@@ -31,17 +31,10 @@
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [],
-    {
-      "name": "HP Pickup",
-      "waypointRelativePos": 0.20476190476190542,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/H Backup.path

@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/HP to 330 Left.path

@@ -3,45 +3,38 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.3072345890410957,
+        "x": 1.174795081967213,
-        "y": 7.135316780821918
+        "y": 7.321618852459016
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.5994434931506847,
+        "x": 2.4670039860768025,
-        "y": 6.909931506849315
+        "y": 7.096233578486413
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 3.973766447368421,
+        "x": 3.6202868852459016,
-        "y": 5.246957236842105
+        "y": 5.859118852459017
       },
       "prevControl": {
-        "x": 3.6582270638067773,
+        "x": 3.304747501684258,
-        "y": 5.772856209444845
+        "y": 6.385017825061756
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Before K"
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [],
-    {
-      "name": "Lift L4",
-      "waypointRelativePos": 0.5,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/J Approach.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.286577868852459,
+        "y": 5.955020491803278
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.166700819672132,
+        "y": 5.679303278688525
+      },
+      "isLocked": false,
+      "linkedName": "Before J"
+    },
+    {
+      "anchor": {
+        "x": 4.974897540983607,
+        "y": 5.115881147540984
+      },
+      "prevControl": {
+        "x": 5.058811475409836,
+        "y": 5.403586065573771
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.71497744813712
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/J Backup.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.988527397260274,
+        "x": 4.974897540983607,
-        "y": 5.227054794520548
+        "y": 5.115881147540984
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.1321837955756875,
+        "x": 5.11855393929902,
-        "y": 5.49468698033176
+        "y": 5.383513333352196
       },
       "isLocked": false,
       "linkedName": "J"
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -48,7 +48,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -121.60750224624898
+    "rotation": -119.71497744813712
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/K Approach.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6202868852459016,
+        "y": 5.859118852459017
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7970635805425386,
+        "y": 5.68234215716238
+      },
+      "isLocked": false,
+      "linkedName": "Before K"
+    },
+    {
+      "anchor": {
+        "x": 3.8720286885245896,
+        "y": 5.043954918032787
+      },
+      "prevControl": {
+        "x": 3.836065573770491,
+        "y": 5.295696721311475
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.94539590092286
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/New Path.path

@@ -45,7 +45,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Right Start to 150 Right.path

@@ -41,7 +41,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -3,45 +3,38 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.587970890410959,
+        "x": 7.588217213114754,
-        "y": 6.143621575342466
+        "y": 7.537397540983607
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.385916095890411,
+        "x": 6.3534836065573765,
-        "y": 6.158647260273973
+        "y": 6.51844262295082
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.988527397260274,
+        "x": 5.286577868852459,
-        "y": 5.227054794520548
+        "y": 5.955020491803278
       },
       "prevControl": {
-        "x": 5.574529109589041,
+        "x": 5.872579581181226,
-        "y": 6.098544520547945
+        "y": 6.826510217830675
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "J"
+      "linkedName": "Before J"
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [],
-    {
-      "name": "Lift L4",
-      "waypointRelativePos": 0.4547619047619047,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -49,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -121.60750224624898
+    "rotation": -119.71497744813712
   },
   "reversed": false,
   "folder": null,

src/main/deploy/pathplanner/paths/Start to H.path

@@ -41,7 +41,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/fein.path

@@ -50,7 +50,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/settings.json

@@ -5,7 +5,7 @@
   "pathFolders": [],
   "autoFolders": [],
   "defaultMaxVel": 4.0,
-  "defaultMaxAccel": 1.0,
+  "defaultMaxAccel": 2.5,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 400.0,
   "defaultNominalVoltage": 12.0,

src/main/java/frc/robot/RobotContainer.java

@@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.events.EventTrigger;
 import com.pathplanner.lib.path.EventMarker;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -154,12 +155,11 @@ public class RobotContainer {
 		
 		driver.start().whileTrue(drivetrain.resetToVision());
 	
-		/* 
 		driver.rightBumper().whileTrue(
 				drivetrain.goToPose(
 					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
 					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
-					() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
+					() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
 				)
 			);
 
@@ -167,10 +167,9 @@ public class RobotContainer {
 			drivetrain.goToPose(
 				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
 				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
-				() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
+				() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
 			)
 		);
-		*/
 
 		//Operator inputs
 		operator.povUp().onTrue(
@@ -227,16 +226,15 @@ public class RobotContainer {
 		);
 
 	}
-
 	private void configureNamedCommands() {
-		new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 		new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
 		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
-		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
+		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
 		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
-		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
 	}
 
 	//creates tabs and transforms them on the shuffleboard
@@ -325,7 +323,7 @@ public class RobotContainer {
 		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
 		apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
 		apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
-
+		apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
 
 //		sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
 

src/main/java/frc/robot/constants/AutoConstants.java

@@ -12,7 +12,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 4;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 1;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 2;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -57,10 +57,10 @@ public class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
-    public static final double kHeadingP = 0.0;
+    public static final double kHeadingP = 0.01;
 
-    public static final double kXTranslationP = 0.0;
+    public static final double kXTranslationP = 0.5;
-    public static final double kYTranslationP = 0.0;
+    public static final double kYTranslationP = 0.5;
 
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

src/main/java/frc/robot/constants/VisionConstants.java

@@ -40,12 +40,12 @@ public class VisionConstants {
         {},
         {},
         {},
-        {4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
+        {13.570, 2.816, 13.858, 2.970},//6
-        {5.789+12.272, 3.862, 5.789+12.272, 4.194},
+        {14.373, 3.862, 14.385, 4.194},
-        {5.275+12.272, 5.075, 4.991+12.272, 5.246},
+        {13.858, 5.032, 13.558, 5.227},
-        {3.986+12.272, 5.24, 3.701+12.272, 5.076},
+        {12.575, 5.227, 12.287, 5.056},
-        {3.183+12.272, 4.191, 3.183, 3.857},
+        {11.772, 4.169, 11.772, 3.845},
-        {3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
+        {12.287, 2.982, 12.587, 2.826},//11
         {},
         {},
         {},
@@ -59,6 +59,6 @@ public class VisionConstants {
         {4.993, 2.816, 5.272, 2.996}
     };
 
-    public static final double latencyFudgeFactor = 0;
+    public static final double latencyFudge = 0;
 
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -22,6 +22,7 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -94,6 +95,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
+        gyro.reset();
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
@@ -104,16 +106,19 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
             },
-            new Pose2d()
+            new Pose2d(),
+            VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
         );
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
-
         pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
         pidHeading.enableContinuousInput(-180, 180);
 
         pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
+        pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
         pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
+        pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
+
 
         AutoBuilder.configure(
             this::getPose, 
@@ -166,7 +171,13 @@ public class Drivetrain extends SubsystemBase {
 
         gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
 
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
+
         if(vision.getOrangeTagDetected()){
+            if(vision.getOrangeDist() < 60){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
+            }
+
             // if the detected tags match your alliances reef tags use their pose estimates  
             if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
                 orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
@@ -180,6 +191,10 @@ public class Drivetrain extends SubsystemBase {
         }
 
         if(vision.getBlackTagDetected()){
+            if(vision.getBlackDist() < 60){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
+            }
+
             if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
                 blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
                 m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 
@@ -191,6 +206,7 @@ public class Drivetrain extends SubsystemBase {
             Logger.recordOutput("black pose", new Pose3d(blackPose2d));
         }
         
+        
         Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
         
         if(musicTimer.get()>10){
@@ -296,10 +312,14 @@ public class Drivetrain extends SubsystemBase {
 
     public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
         return run(() -> {
-            drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
+            drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
-                pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), 
+                MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1), 
                 pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), 
                 true);
+
+                Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
+                    new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
+                    new Rotation2d(headingSetpoint.getAsDouble()))));
             });
     }
 
@@ -307,12 +327,12 @@ public class Drivetrain extends SubsystemBase {
 
 		if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
 			int closestTag = 17;
-			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][0]), 2)
-			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
+			+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][1]), 2));
 			
 			for(int i = 17; i <= 22; ++i){
-				double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+				double distance = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][0]), 2)
-				+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
+				+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
 
 				if(distance < closestTagDist){
 					closestTag = i;
@@ -322,12 +342,12 @@ public class Drivetrain extends SubsystemBase {
 			return closestTag;
 		}else{
 			int closestTag = 6;
-			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+			double closestTagDist = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][0]), 2)
-			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
+			+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][1]), 2));
 			
 			for(int i = 6; i <= 11; ++i){
-				double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+				double distance = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters( VisionConstants.globalTagCoords[i][0]), 2)
-				+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
+				+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
 
 				if(distance < closestTagDist){
 					closestTag = i;

src/main/java/frc/robot/subsystems/Vision.java

@@ -15,6 +15,8 @@ import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
+    private NetworkTable blackVisionTable;
+
     private DoubleSubscriber black_tx;
     private DoubleSubscriber black_ty;
     private DoubleSubscriber black_dist;
@@ -24,6 +26,8 @@ public class Vision{
 
     private DoubleSubscriber blackFramerate;
 
+    private NetworkTable orangeVisionTable;
+
     private DoubleSubscriber orange_tx;
     private DoubleSubscriber orange_ty;
     private DoubleSubscriber orange_dist;
@@ -39,8 +43,8 @@ public class Vision{
     public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
-        NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
+        blackVisionTable = inst.getTable("black_Fiducial");
-        NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
+        orangeVisionTable = inst.getTable("orange_Fiducial");
 
         black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
         black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
@@ -103,7 +107,7 @@ public class Vision{
 
     public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
-            getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
+            getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
     }
 
     public double getBlackTX(){
@@ -123,7 +127,7 @@ public class Vision{
     }
 
     public double getBlackTimeStamp(){
-        return black_tx.getLastChange();
+        return black_tx.getLastChange()-VisionConstants.latencyFudge;
     }
 
     public boolean getBlackTagDetected(){
@@ -137,7 +141,7 @@ public class Vision{
     public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
             return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
-                orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
+                orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
                 );
         }else{
             return new Pose2d();
@@ -161,7 +165,7 @@ public class Vision{
     }
 
     public double getOrangeTimeStamp(){
-        return orange_tx.getLastChange();
+        return orange_tx.getLastChange()-VisionConstants.latencyFudge;
     }
 
     public boolean getOrangeTagDetected(){