Cleaned up test code

Robocubs · Feb 6, 2024 · b313ce3 · b313ce3
1 parent 0e42fa5
commit b313ce3
Show file tree

Hide file tree

Showing 7 changed files with 77 additions and 529 deletions.
diff --git a/src/main/java/com/team1701/lib/drivers/motors/MotorIOSparkFlex.java b/src/main/java/com/team1701/lib/drivers/motors/MotorIOSparkFlex.java
@@ -29,21 +29,36 @@ public MotorIOSparkFlex(CANSparkFlex motor, double reduction) {
 
     @Override
     public void updateInputs(MotorInputs inputs) {
-        inputs.positionRadians = Units.rotationsToRadians(mEncoder.getPosition()) * mReduction;
-        inputs.velocityRadiansPerSecond =
-                Units.rotationsPerMinuteToRadiansPerSecond(mEncoder.getVelocity()) * mReduction;
-        mPositionSamples.ifPresent(samples -> {
-            inputs.positionRadiansSamples = samples.stream()
-                    .mapToDouble((position) -> Units.rotationsToRadians(position) * mReduction)
-                    .toArray();
-            samples.clear();
-        });
-        mVelocitySamples.ifPresent(samples -> {
-            inputs.velocityRadiansPerSecondSamples = samples.stream()
-                    .mapToDouble((velocity) -> Units.rotationsPerMinuteToRadiansPerSecond(velocity) * mReduction)
-                    .toArray();
-            samples.clear();
-        });
+        mPositionSamples.ifPresentOrElse(
+                (samples) -> {
+                    inputs.positionRadiansSamples =
+                            samples.stream().mapToDouble(this::toReducedRadians).toArray();
+                    inputs.positionRadians = inputs.positionRadiansSamples.length > 0
+                            ? inputs.positionRadiansSamples[inputs.positionRadiansSamples.length - 1]
+                            : toReducedRadians(mEncoder.getPosition());
+                    samples.clear();
+                },
+                () -> inputs.positionRadians = toReducedRadians(mEncoder.getPosition()));
+
+        mVelocitySamples.ifPresentOrElse(
+                (samples) -> {
+                    inputs.velocityRadiansPerSecondSamples = samples.stream()
+                            .mapToDouble(this::toReducedRadiansPerSecond)
+                            .toArray();
+                    inputs.velocityRadiansPerSecond = inputs.velocityRadiansPerSecondSamples.length > 0
+                            ? inputs.velocityRadiansPerSecondSamples[inputs.velocityRadiansPerSecondSamples.length - 1]
+                            : toReducedRadiansPerSecond(mEncoder.getVelocity());
+                    samples.clear();
+                },
+                () -> inputs.velocityRadiansPerSecond = toReducedRadiansPerSecond(mEncoder.getVelocity()));
+    }
+
+    private double toReducedRadians(double encoderPosition) {
+        return Units.rotationsToRadians(encoderPosition) * mReduction;
+    }
+
+    private double toReducedRadiansPerSecond(double encoderVelocity) {
+        return Units.rotationsPerMinuteToRadiansPerSecond(encoderVelocity) * mReduction;
     }
 
     @Override

diff --git a/...am1701/lib/estimation/PoseEstimator3.java → ...eam1701/lib/estimation/PoseEstimator.java b/...am1701/lib/estimation/PoseEstimator3.java → ...eam1701/lib/estimation/PoseEstimator.java
@@ -7,26 +7,37 @@
 import java.util.TreeMap;
 
 import com.team1701.lib.alerts.Alert;
+import com.team1701.lib.swerve.ExtendedSwerveDriveKinematics;
 import com.team1701.lib.util.GeometryUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Timer;
 
-public class PoseEstimator3 {
+public class PoseEstimator {
     private static final double kHistorySeconds = 0.5;
 
     private final Alert mVisionAlert = Alert.error("Vision measurements added before drive measurements.");
     private final Matrix<N3, N1> mQ = new Matrix<>(Nat.N3(), Nat.N1());
     private final NavigableMap<Double, PoseUpdate> mUpdates = new TreeMap<>();
+    private final ExtendedSwerveDriveKinematics mKinematics;
 
     private Pose2d mPose = GeometryUtil.kPoseIdentity;
+    private DriveMeasurement mLastDriveMeasurement;
+
+    public static record DriveMeasurement(
+            double timestampSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
+        public DriveMeasurement(double timestampSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+            this(timestampSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+        }
 
-    public static record DriveMeasurement(double timestampSeconds, Twist2d twist2d) {
         private static final Comparator<DriveMeasurement> compareTimestamp =
                 (DriveMeasurement a, DriveMeasurement b) -> Double.compare(a.timestampSeconds, b.timestampSeconds);
     }
@@ -38,7 +49,13 @@ public static record VisionMeasurement(double timestampSeconds, Pose2d pose, Mat
                         b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0));
     }
 
-    public PoseEstimator3(Matrix<N3, N1> stateStdDevs) {
+    public PoseEstimator(ExtendedSwerveDriveKinematics kinematics, Matrix<N3, N1> stateStdDevs) {
+        mKinematics = kinematics;
+
+        var modulePositions = new SwerveModulePosition[kinematics.getNumModules()];
+        Arrays.fill(modulePositions, new SwerveModulePosition());
+        mLastDriveMeasurement = new DriveMeasurement(0.0, Rotation2d.fromDegrees(0.0), modulePositions);
+
         for (int i = 0; i < 3; ++i) {
             mQ.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
         }
@@ -69,16 +86,16 @@ public void addDriveMeasurements(DriveMeasurement... measurements) {
 
         // Add new data
         var previousUpdate = mUpdates.floorEntry(measurements[0].timestampSeconds);
-        var basePose = previousUpdate == null
+        mPose = previousUpdate == null
                 ? mPose
                 : previousUpdate.getValue().basePose.exp(previousUpdate.getValue().twist);
         for (var measurement : measurements) {
-            mUpdates.put(
-                    measurement.timestampSeconds, new PoseUpdate(basePose, measurement.twist2d, new ArrayList<>()));
-            basePose = basePose.exp(measurement.twist2d);
-        }
+            var twist2d = mKinematics.toTwist2d(mLastDriveMeasurement.wheelPositions, measurement.wheelPositions);
+            mLastDriveMeasurement = measurement;
 
-        mPose = basePose;
+            mUpdates.put(measurement.timestampSeconds, new PoseUpdate(mPose, twist2d, new ArrayList<>()));
+            mPose = mPose.exp(twist2d);
+        }
 
         // Clear old data
         while (mUpdates.size() > 1 && mUpdates.firstKey() < Timer.getFPGATimestamp() - kHistorySeconds) {

diff --git a/src/main/java/com/team1701/lib/estimation/PoseEstimator1.java b/src/main/java/com/team1701/lib/estimation/PoseEstimator1.java