From fa409fac81df997dacc92490f301d5031816c53f Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 28 Jan 2024 13:29:53 -0800 Subject: [PATCH 1/6] switched degrees to rotations --- .../robot/subsystems/drive/module/Module.java | 4 +-- .../subsystems/drive/module/ModuleIO.java | 2 +- .../subsystems/drive/module/ModuleIOSim.java | 2 +- .../drive/module/ModuleIOTalonFX.java | 26 ++++++------------- 4 files changed, 12 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 7ecf28b4..0358b95e 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -90,7 +90,7 @@ public void periodic() { // Run closed loop turn control if (m_angleSetpoint != null) { - m_io.setTurnPositionDegs(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations()); + m_io.setTurnPositionRots(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations()); // Run closed loop drive control // Only allowed if closed loop turn control is running @@ -149,7 +149,7 @@ public void runCharacterization(double volts) { /** Disables all outputs to motors. */ public void stop() { - m_io.setTurnPositionDegs(0.0); + m_io.setTurnPositionRots(0.0); m_io.setDriveVelocityMPS(0.0); // Disable closed loop control for turn and drive diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index 6b5bac51..d79dab59 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -129,7 +129,7 @@ default void updateInputs(ModuleIOInputs inputs) {} default void setDriveVelocityMPS(double mps) {} /** Run the turn motor at the specified voltage. */ - default void setTurnPositionDegs(double degrees) {} + default void setTurnPositionRots(double degrees) {} /** Enable or disable brake mode on the drive motor. */ default void setDriveBrakeMode(boolean enable) {} diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index b3a15eb7..1a7ab440 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -104,7 +104,7 @@ public void setDriveVelocityMPS(double mps) { } @Override - public void setTurnPositionDegs(double degrees) { + public void setTurnPositionRots(double degrees) { double rots = degrees / 360; double volts = m_turnPid.calculate(m_turnSim.getAngularPositionRotations(), rots); turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 583332c1..bfc53659 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -199,9 +199,9 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnPid.updateIfChanged(); inputs.drivePositionRad = - Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); + Units.rotationsToRadians(m_drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); + Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()}; @@ -210,28 +210,19 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.setTurnPosition(Rotation2d.fromRotations(( m_turnPosition.getValueAsDouble()))); inputs.setTurnVelocityRadPerSec( - Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); + Units.rotationsToRadians(m_turnVelocity.getValueAsDouble())); inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble()); inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()}); inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO()) + .mapToDouble(Units::rotationsToRadians) .toArray()); inputs.setOdometryTurnPositions(m_turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO())) + .map(Rotation2d::fromRotations) .toArray(Rotation2d[]::new)); + m_drivePositionQueue.clear(); m_turnPositionQueue.clear(); - - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Closed Loop Output", m_turnClosedLoopOutput.getValueAsDouble()); - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Closed Loop Error", m_turnClosedLoopError.getValueAsDouble()); - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Closed Loop Reference", m_turnClosedLoopReference.getValueAsDouble()); - - SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() + - "/Recorded kP", m_turnPid.getSlotConfigs().kP); } @Override @@ -242,9 +233,8 @@ public void setDriveVelocityMPS(double mps) { } @Override - public void setTurnPositionDegs(double degrees) { -// double rots = degrees / 360; - m_turnTalon.setControl(m_posRequest.withPosition(degrees)); + public void setTurnPositionRots(double rotations) { + m_turnTalon.setControl(m_posRequest.withPosition(rotations)); } @Override From efa6793818dc6e7f4816690dbb6d981aa6b55753 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 28 Jan 2024 14:12:33 -0800 Subject: [PATCH 2/6] updated offsets --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 725694a8..c5a9a6c9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -61,7 +61,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.017578), // offset 0.457764 + Units.rotationsToDegrees(0.230713), // offset 0.457764 true, // inversion ModuleConstants.GearRatios.L3 ); @@ -72,7 +72,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.201172), + Units.rotationsToDegrees(0.203857), true, ModuleConstants.GearRatios.L3 ); @@ -83,7 +83,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.457764), + Units.rotationsToDegrees(0.212158), true, ModuleConstants.GearRatios.L3 ); @@ -94,7 +94,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.263916), + Units.rotationsToDegrees(-0.263184), true, ModuleConstants.GearRatios.L3 ); From c0f945e4b5c479af46fda197a3d17f5fb728db75 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 31 Jan 2024 15:54:42 -0800 Subject: [PATCH 3/6] changed sim crap --- build.gradle | 2 +- networktables.json | 48 ++++++++++++++++++++++++++ networktables.json.bck | 32 +++++++++++++++++ src/main/java/frc/robot/Constants.java | 2 +- 4 files changed, 82 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 7d3cee78..6deddbf8 100644 --- a/build.gradle +++ b/build.gradle @@ -116,7 +116,7 @@ sonar { // AdvantageKit log replay from the command line. Set the // value to "true" to enable the sim GUI by default (this // is the standard WPILib behavior). -wpi.sim.addGui().defaultEnabled = true +wpi.sim.addGui() wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') diff --git a/networktables.json b/networktables.json index 6cc7f9ce..1e88e928 100644 --- a/networktables.json +++ b/networktables.json @@ -126,5 +126,53 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Sim Turn PID.mm.kp", + "type": "double", + "value": 0.1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Turn PID.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Turn PID.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Drive PID.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Drive PID.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Sim Drive PID.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index 4dc34172..6cc7f9ce 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -94,5 +94,37 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kp", + "type": "double", + "value": 0.05, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", + "type": "double", + "value": 7.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kd", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } } ] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c5a9a6c9..94c15698 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ public final class Constants { private Constants() { throw new IllegalStateException("Constants class should not be constructed"); } - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public enum Mode { /** Running on a real robot. */ From 18efea6719455d623bf8bf1e7a443da794576655 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 3 Feb 2024 11:13:16 -0500 Subject: [PATCH 4/6] fixed some logic with sonarqube --- .../robot/subsystems/drive/module/Module.java | 41 ++----------------- .../drive/module/ModuleIOTalonFX.java | 21 ++++++---- .../drive/module/PhoenixOdometryThread.java | 1 + .../phoenix6/Phoenix6PidPropertyBuilder.java | 7 ++-- .../phoenix6/PidPropertyPublic.java | 22 +++++----- 5 files changed, 31 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 0358b95e..c5f3f3eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -28,12 +28,8 @@ public class Module { private final ModuleIO m_io; private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); - private final ModuleConstants m_moduleConstants; private final int m_index; - private final SimpleMotorFeedforward m_driveFeedforward; - private final PIDController m_driveFeedback; - private final PIDController m_turnFeedback; private Rotation2d m_angleSetpoint = null; // Setpoint for closed loop control, null for open loop private Double m_speedSetpoint = null; // Setpoint for closed loop control, null for open loop private Rotation2d m_turnRelativeOffset = null; // Relative + Offset = Absolute @@ -42,32 +38,11 @@ public class Module { public Module(ModuleIO io) { this.m_io = io; - this.m_moduleConstants = m_io.getModuleConstants(); - this.m_index = m_moduleConstants.MODULE_INDEX(); + ModuleConstants moduleConstants = m_io.getModuleConstants(); + this.m_index = moduleConstants.MODULE_INDEX(); // delay to initialize all hardware Timer.delay(0.5); - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL, REPLAY -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - m_driveFeedback = new PIDController(0.05, 0.0, 0.0); - m_turnFeedback = new PIDController(0.1, 0.0, 0.0); - } - case SIM -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - m_driveFeedback = new PIDController(0.1, 0.0, 0.0); - m_turnFeedback = new PIDController(10.0, 0.0, 0.0); - } - default -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - m_driveFeedback = new PIDController(0.0, 0.0, 0.0); - m_turnFeedback = new PIDController(0.0, 0.0, 0.0); - } - } - - m_turnFeedback.enableContinuousInput(-Math.PI, Math.PI); setBrakeMode(true); } @@ -95,18 +70,8 @@ public void periodic() { // Run closed loop drive control // Only allowed if closed loop turn control is running if (m_speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = m_speedSetpoint * Math.cos(m_turnFeedback.getPositionError()); - // Run drive controller - m_io.setDriveVelocityMPS(adjustSpeedSetpoint); -// m_io.setDriveVelocityMPS( -// m_driveFeedforward.calculate(velocityRadPerSec) -// + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec)); + m_io.setDriveVelocityMPS(m_speedSetpoint); } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index bfc53659..e5bd9c6c 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -79,7 +79,17 @@ public class ModuleIOTalonFX implements ModuleIO { public ModuleIOTalonFX(ModuleConstants moduleConstants) { m_driveTalon = new TalonFX(moduleConstants.DRIVE_MOTOR_ID()); m_turnTalon = new TalonFX(moduleConstants.TURN_MOTOR_ID()); - CANcoder cancoder = new CANcoder(moduleConstants.ENCODER_ID()); + try (CANcoder cancoder = new CANcoder(moduleConstants.ENCODER_ID())) { + // run factory default on cancoder + var encoderConfig = new CANcoderConfiguration(); + encoderConfig.MagnetSensor.SensorDirection = + moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + cancoder.getConfigurator().apply(encoderConfig); + + m_turnAbsolutePosition = cancoder.getAbsolutePosition(); + } m_moduleConstants = moduleConstants; @@ -128,13 +138,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { m_posRequest = new PositionVoltage(0, 0, false, 0, 0, false, false, false); - // run factory default on cancoder - var encoderConfig = new CANcoderConfiguration(); - encoderConfig.MagnetSensor.SensorDirection = - moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; - encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - cancoder.getConfigurator().apply(encoderConfig); + // Fancy multithreaded odometry update stuff // setup drive values @@ -148,7 +152,6 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { // setup turn values m_turnTalon.setPosition(0.0); - m_turnAbsolutePosition = cancoder.getAbsolutePosition(); m_turnPosition = m_turnTalon.getPosition(); m_turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(m_turnTalon, m_turnTalon.getPosition()); diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index 2a6bde0a..eb9cb8d2 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -95,6 +95,7 @@ public void run() { } } catch (InterruptedException e) { DriverStation.reportError(e.toString(), true); + Thread.currentThread().interrupt(); } finally { signalsLock.unlock(); } diff --git a/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java b/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java index fafc623a..c0ffafc0 100644 --- a/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java +++ b/src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.hardware.core.CoreTalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; -import com.gos.lib.properties.BaseHeavyProperty; import com.gos.lib.properties.GosDoubleProperty; import com.gos.lib.properties.HeavyDoubleProperty; @@ -15,7 +14,7 @@ import java.util.function.DoubleConsumer; public final class Phoenix6PidPropertyBuilder { - private final CoreTalonFX m_motor; + private final CoreTalonFX m_motor; private final SlotConfigs m_slot; private final boolean m_isConstant; private final String m_baseName; @@ -42,7 +41,9 @@ public Phoenix6PidPropertyBuilder(String baseName, boolean isConstant, CoreTalon } } - private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix, double defaultValue, DoubleConsumer setter) { + private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix, + double defaultValue, + DoubleConsumer setter) { String propertyName = m_baseName + ".mm." + propertyNameSuffix; GosDoubleProperty prop = new GosDoubleProperty(m_isConstant, propertyName, defaultValue); diff --git a/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java b/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java index b9529f39..7c720646 100644 --- a/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java +++ b/src/main/java/lib/properties/phoenix6/PidPropertyPublic.java @@ -5,19 +5,19 @@ import java.util.List; public class PidPropertyPublic { - private final List m_properties; + private final List m_properties; - public PidPropertyPublic(List properties) { - m_properties = properties; - } + public PidPropertyPublic(List properties) { + m_properties = properties; + } - public final void updateIfChanged() { - updateIfChanged(false); - } + public final void updateIfChanged() { + updateIfChanged(false); + } - public final void updateIfChanged(boolean forceUpdate) { - for (HeavyDoubleProperty property : m_properties) { - property.updateIfChanged(forceUpdate); - } + public final void updateIfChanged(boolean forceUpdate) { + for (HeavyDoubleProperty property : m_properties) { + property.updateIfChanged(forceUpdate); } + } } From bf31570fa57d3888cb83073a8e31e0022f866cc3 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 3 Feb 2024 11:23:29 -0500 Subject: [PATCH 5/6] optimized imports --- src/main/java/frc/robot/subsystems/drive/module/Module.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index c5f3f3eb..2a90d2b1 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -13,8 +13,6 @@ package frc.robot.subsystems.drive.module; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; From 73047d13a0b127d6b63cb6690017efc0f1a2b92f Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Thu, 8 Feb 2024 21:52:05 -0500 Subject: [PATCH 6/6] Fix: Removed Unused import statements --- .../frc/robot/subsystems/drive/module/ModuleIOTalonFX.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 6ae24c9d..291f87e7 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -28,10 +28,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; -import lib.properties.phoenix6.PidPropertyPublic; -import org.littletonrobotics.junction.Logger; import java.util.Queue;