Merge branch 'main' of https://github.com/Galaxia5987/Common

src/main/java/frc/robot/swerve/SwerveDrive.java

@@ -13,15 +13,16 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import lib.PoseEstimation;
 import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.Collections;
 import java.util.List;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import java.util.stream.Stream;
+import lib.PoseEstimation;
 import lib.controllers.DieterController;
 import lib.math.differential.Derivative;
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -35,7 +36,7 @@ public class SwerveDrive extends SubsystemBase {
     @AutoLogOutput
     private final SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
 
-    private final List<SwerveModulePosition[]> highFreqModulePositions = new ArrayList<>();
+    private final List<SwerveModulePosition[]> highFreqModulePositions = new ArrayList<>(Collections.singleton(modulePositions));
 
     private final GyroIO gyro;
     private final SwerveDriveKinematics kinematics =
@@ -59,7 +60,7 @@ private SwerveDrive(GyroIO gyroIO, double[] wheelOffsets, ModuleIO... moduleIOs)
 
         updateModulePositions();
 
-        poseEstimator = new PoseEstimation();
+        poseEstimator = new PoseEstimation(this);
     }
 
     public static SwerveDrive getInstance() {

src/main/java/lib/PoseEstimation.java

@@ -9,12 +9,13 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 
 public class PoseEstimation {
-    private static PoseEstimation INSTANCE = null;
     private final SwerveDrivePoseEstimator estimator;
-    private final Vision vision = Vision.getInstance();
-    private final SwerveDrive swerveDrive = SwerveDrive.getInstance();
+    private final SwerveDrive swerveDrive;
+    private final Vision vision;
 
-    public PoseEstimation() {
+    public PoseEstimation(SwerveDrive swerveDrive) {
+        this.swerveDrive = swerveDrive;
+        vision = Vision.getInstance();
         estimator =
                 new SwerveDrivePoseEstimator(
                         swerveDrive.getKinematics(),
@@ -23,13 +24,6 @@ public PoseEstimation() {
                         swerveDrive.getBotPose());
     }
 
-    public static PoseEstimation getInstance() {
-        if (INSTANCE == null) {
-            INSTANCE = new PoseEstimation();
-        }
-        return INSTANCE;
-    }
-
     public void addVisionMeasurement(
             DoubleSupplier std1, DoubleSupplier std2, DoubleSupplier std3) {
         var results = vision.getResults();

src/main/java/lib/Utils.java

@@ -5,7 +5,6 @@
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
-
 import java.util.Comparator;
 import java.util.List;