diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/GyroIOPigeon2.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/Gyro/GyroIOPigeon2.java new file mode 100644 index 0000000..8cbeaed --- /dev/null +++ b/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 yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal 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(); + } +} \ No newline at end of file