From 73da7139e130337038490615cdfe853375a14853 Mon Sep 17 00:00:00 2001
From: PotmanNob <Potmannob@outlook.com>
Date: Fri, 8 Nov 2024 15:17:03 -0600
Subject: [PATCH] ctrl+c ctrl+v

---
 .../bumSwerve/Gyro/SwerveGyroIOPigeon2.java   | 61 +++++++++++++++++++
 1 file changed, 61 insertions(+)
 create mode 100644 src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/SwerveGyroIOPigeon2.java

diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/SwerveGyroIOPigeon2.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/SwerveGyroIOPigeon2.java
new file mode 100644
index 0000000..8715f97
--- /dev/null
+++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/SwerveGyroIOPigeon2.java
@@ -0,0 +1,61 @@
+package frc.robot.pioneersLib.bumSwerve.Gyro;
+
+import java.util.OptionalDouble;
+import java.util.Queue;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import frc.robot.pioneersLib.bumSwerve.OdometryThread;
+
+public class SwerveGyroIOPigeon2 implements SwerveGyroIO{
+
+    private final Pigeon2 gyro;
+	private final Queue<Double> yawPositionQueue;
+	private final Queue<Double> yawTimestampQueue;
+    private Rotation3d offset;
+
+    public SwerveGyroIOPigeon2(int canId) {
+        gyro = new Pigeon2(canId);
+
+        gyro.reset();
+        offset = new Rotation3d();
+
+        yawTimestampQueue = OdometryThread.getInstance().makeTimestampQueue();
+		yawPositionQueue = OdometryThread.getInstance()
+            .registerSignal(() -> {
+                if(gyro.getUpTime().getValue() != 0) {
+                    return OptionalDouble.of(gyro.getRotation3d().minus(offset).getAngle());
+                } else {
+                    return OptionalDouble.empty();
+                }
+            });
+    }
+
+    @Override
+    public void zero() {
+        this.offset = gyro.getRotation3d();
+    }
+
+    @Override
+	public void updateInputs(SwerveGyroIOInputs inputs) {
+		inputs.connected = (gyro.getUpTime().getValue() != 0);
+		inputs.yawPosition = Rotation2d.fromRadians(gyro.getRotation3d().minus(offset).getAngle());
+		inputs.yawVelocityRPS = gyro.getRate();
+
+		if (yawTimestampQueue != null && yawPositionQueue != null) {
+			inputs.odometryYawTimestamps = yawTimestampQueue
+				.stream()
+				.mapToDouble((Double value) -> value)
+				.toArray();
+			inputs.odometryYawPositions = yawPositionQueue
+				.stream()
+				.map((Double value) -> Rotation2d.fromDegrees(value))
+				.toArray(Rotation2d[]::new);
+
+			yawTimestampQueue.clear();
+			yawPositionQueue.clear();
+		}
+	}
+}