001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.subsystems.swerve;
006
007import org.wpilib.math.geometry.Rotation2d;
008import org.wpilib.units.measure.Angle;
009import org.wpilib.units.measure.AngularVelocity;
010
011import com.ctre.phoenix6.BaseStatusSignal;
012import com.ctre.phoenix6.StatusCode;
013import com.ctre.phoenix6.StatusSignal;
014import com.ctre.phoenix6.configs.Pigeon2Configuration;
015import com.ctre.phoenix6.hardware.Pigeon2;
016
017import java.util.Queue;
018
019/** IO implementation for Pigeon 2. */
020public class GyroIOPigeon2 implements GyroIO {
021    private final Pigeon2 pigeon =
022            new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, TunerConstants.kCANBus);
023    private final StatusSignal<Angle> yaw = pigeon.getYaw();
024    private final Queue<Double> yawPositionQueue;
025    private final Queue<Double> yawTimestampQueue;
026    private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
027
028    public GyroIOPigeon2() {
029        if (TunerConstants.DrivetrainConstants.Pigeon2Configs != null) {
030            pigeon.getConfigurator().apply(TunerConstants.DrivetrainConstants.Pigeon2Configs);
031        } else {
032            pigeon.getConfigurator().apply(new Pigeon2Configuration());
033        }
034
035        pigeon.getConfigurator().setYaw(0.0);
036        yaw.setUpdateFrequency(Swerve.ODOMETRY_FREQUENCY);
037        yawVelocity.setUpdateFrequency(50.0);
038        pigeon.optimizeBusUtilization();
039        yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
040        yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone());
041    }
042
043    @Override
044    public void updateInputs(GyroIOInputs inputs) {
045        inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
046        inputs.yawPosition = yaw.getValue();
047        inputs.yawVelocityRadPerSec = yawVelocity.getValue();
048
049        inputs.odometryYawTimestamps =
050                yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
051        inputs.odometryYawPositions =
052                yawPositionQueue.stream()
053                        .map((Double value) -> Rotation2d.fromDegrees(value))
054                        .toArray(Rotation2d[]::new);
055        yawTimestampQueue.clear();
056        yawPositionQueue.clear();
057    }
058}