Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code

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

@@ -63,7 +63,7 @@ public class RobotContainer {
 
 		drivetrain = new Drivetrain();
 
-		vision = new Vision(drivetrain::getGyroValue);
+		vision = new Vision();
 
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();

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

@@ -19,6 +19,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.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -45,6 +46,8 @@ public class Drivetrain extends SubsystemBase {
 
     // Odometry class for tracking robot pose
     private SwerveDrivePoseEstimator m_estimator;
+    
+    private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
 
     public Orchestra m_orchestra = new Orchestra();
     private Timer musicTimer = new Timer();
@@ -144,6 +147,8 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
         });
 
+        gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp());
+
 
         // if the detected tags match your alliances reef tags use their pose estimates 
         /* 
@@ -351,6 +356,10 @@ public class Drivetrain extends SubsystemBase {
         return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
     }
 
+    public TimeInterpolatableBuffer<Double> getGyroBuffer(){
+        return gyroBuffer;
+    }
+
     /**
      * Returns the turn rate of the robot.
      *

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

@@ -2,10 +2,12 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
@@ -32,9 +34,10 @@ public class Vision{
 
     private DoubleSubscriber orangeFramerate;
 
-    private DoubleSupplier gyroAngle;
+    private double[] orangeCamPose = {0,0,0,0,0};
+    private double[] blackCamPose = {0,0,0,0,0};
 
-    public Vision(DoubleSupplier gyroAngle){
+    public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
@@ -57,23 +60,45 @@ public class Vision{
         orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
 
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
+    
     }
     
-    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
         Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
             VisionConstants.globalTagCoords[tagID][1],
             new Rotation2d());
 
-        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
 
         Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
 
         Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
 
-        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
     }
 
-    public Pose2d getBlackGlobalPose(){
+    public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
+        double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty);
+
+        Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx));
+
+        Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
+
+        Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
+            .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
+            .getTranslation();
+        Pose2d robotPose = new Pose2d(fieldToCameraTranslation,
+            new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0])))
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(orangeCamPose[0])), Pose2d.kZero));
+
+        robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get()));
+
+        return robotPose;
+    }
+
+    public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         return relativeToGlobalPose2d(getBlackClosestTag(), 
             new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
     }