Pigeon gyro

src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/GyroIOPigeon2.java

@@ -0,0 +1,57 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+package frc.robot.pioneersLib.bumSwerve.Gyro;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.pioneersLib.bumSwerve.OdometryThread;
+
+import java.util.Queue;
+
+public class GyroIOPigeon2 implements SwerveGyroIO {
+  private Pigeon2 pigeon;
+  private final StatusSignal<Double> yaw = pigeon.getYaw();
+  private final Queue<Double> yawPositionQueue;
+  private final Queue<Double> yawTimestampQueue;
+  private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZWorld();
+
+  public GyroIOPigeon2(int CANId) {
+    pigeon = new Pigeon2(CANId);
+    pigeon.getConfigurator().apply(new Pigeon2Configuration());
+    pigeon.getConfigurator().setYaw(0.0);
+    yaw.setUpdateFrequency(250);
+    yawVelocity.setUpdateFrequency(100.0);
+    pigeon.optimizeBusUtilization();
+
+    yawTimestampQueue = OdometryThread.getInstance().makeTimestampQueue();
+    yawPositionQueue =
+        OdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw());
+
+  }
+
+  @Override
+  public void updateInputs(SwerveGyroIOInputs inputs) {
+    inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+
+    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();
+  }
+}