edited comments

Team766 · Jan 2, 2025 · 2456d80 · 2456d80
1 parent 6a0d530
commit 2456d80
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 26 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -33,5 +33,6 @@
   "diffEditor.ignoreTrimWhitespace": false,
   "java.format.settings.url": "https://raw.githubusercontent.com/google/styleguide/gh-pages/eclipse-java-google-style.xml",
   "java.checkstyle.configuration": "${workspaceFolder}/.github/linters/checkstyle.xml",
-  "java.checkstyle.version": "9.3"
+  "java.checkstyle.version": "9.3",
+  "java.debug.settings.onBuildFailureProceed": true
 }
diff --git a/src/main/java/com/team766/odometry/KalmanFilter.java b/src/main/java/com/team766/odometry/KalmanFilter.java
@@ -20,26 +20,26 @@ public class KalmanFilter {
     private Matrix<N4, N4> noiseCovariance;
     private Matrix<N2, N2> odometryCovariancePerDist;
     private Matrix<N2, N2> visionCovariance;
-    private TreeMap<Double, Translation2d> inputLog; 
+    private TreeMap<Double, Translation2d> inputLog; // TODO: make circular buffer
     private double velocityInputDeletionTime; // in seconds
 
     private static final Matrix<N4, N1> CUR_STATE_DEFAULT = MatBuilder.fill(Nat.N4(), Nat.N1(), 0, 0, 0, 0);
 
     private static final Matrix<N4, N4> COVARIANCE_DEFAULT = Matrix.eye(Nat.N4());
 
     private static final Matrix<N4, N4> NOISE_COVARIANCE_DEFAULT = MatBuilder.fill(Nat.N4(), Nat.N4(), 
-        0.03, 0, 0, 0,
-                0, 0.03, 0, 0,
-                0, 0, 0.01, 0,
-                0, 0, 0, 0.01);
+        0.3, 0, 0, 0,
+                0, 0.3, 0, 0,
+                0, 0, 0.1, 0,
+                0, 0, 0, 0.1);
 
     private static final Matrix<N2, N2> ODOMETRY_COVARIANCE_DEFAULT = MatBuilder.fill(Nat.N2(), Nat.N2(), 
         0.2, 0, 
                 0, 0.05);
 
     private static final Matrix<N2, N2> VISION_COVARIANCE_DEFAULT = MatBuilder.fill(Nat.N2(), Nat.N2(), 
-        0.1, 0, 
-                0, 0.1);
+        0.2, 0, 
+                0, 0.2);
 
     private static final Matrix<N2, N4> OBSERVATION_MATRIX = MatBuilder.fill(Nat.N2(), Nat.N4(), 
         1, 0, 0, 0,
@@ -118,8 +118,8 @@ private void findPrevState(double targetTime) {
         double prevTime;
         double dt; 
 
-        while (time > targetTime) {
-            try {
+        while (time > targetTime) { 
+            try { // TODO: use something other than try catch
                 prevTime = inputLog.lowerKey(time);
                 dt = Math.max(prevTime, targetTime) - time; // will be negative
 
@@ -192,8 +192,8 @@ private void updateWithPositionMeasurement(Translation2d measurement, Matrix<N2,
     }
 
     /**
-     * Updates the estimated position using a 
-     * @param measurement
+     * Updates the estimated position using a vision measurement from a given time
+     * @param measurement measured position on the field
      * @param time in seconds
      */
     public void updateWithVisionMeasurement(Translation2d measurement, double time) {

diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java
@@ -297,13 +297,6 @@ public void controlFieldOrientedWithRotationTarget(double x, double y, Rotation2
         movingToTarget = true;
         this.x = x;
         this.y = y;
-
-        // controlFieldOriented(
-        //         x,
-        //         y,
-        //         (Math.abs(rotationPID.getOutput()) < ControlConstants.DEFAULT_ROTATION_THRESHOLD
-        //                 ? 0
-        //                 : rotationPID.getOutput()));
     }
 
     public boolean isAtRotationTarget() {
@@ -404,12 +397,10 @@ public Pose2d getCurrentPosition() {
 
     public void setCurrentPosition(Pose2d P) {
         kalmanFilter.setPos(P.getTranslation());
-        // log("setCurrentPosition(): " + P);
     }
 
     public void resetCurrentPosition() {
         kalmanFilter.setPos(new Translation2d());
-        // swerveOdometry.setCurrentPosition(new Pose2d());
     }
 
     /**
@@ -469,7 +460,6 @@ public void run() {
         }
 
         // SmartDashboard.putBoolean("movingToTarget", movingToTarget);
-
         // SmartDashboard.putBoolean("isAtRotationTarget", isAtRotationTarget());
 
         swerveFR.dashboardCurrentUsage();
@@ -513,14 +503,15 @@ public void runSim() {
             simPrevPoses.remove(simPrevPoses.firstKey()); // delete old values
         } 
 
-        // kalmanFilter.updateWithOdometry(swerveOdometry.predictCurrentPositionChange(), now - dt, now);
+        kalmanFilter.updateWithOdometry(swerveOdometry.predictCurrentPositionChange());
 
-        if (Math.random() < 0.5) {
-            double delay = 0.05; // Math.random() * 0.5;
+        if (Math.random() < 0.5) { // simulate inconsistent vision updates
+            double delay = 0.05; 
             Pose2d prevPose = simPrevPoses.ceilingEntry(now - delay).getValue();
+            // simulated vision position is randomly chosen in an area around actual position
             double randX = prevPose.getX() + 0.1 * (Math.random() - 0.5);
             double randY = prevPose.getY() + 0.1 * (Math.random() - 0.5);
-            kalmanFilter.updateWithVisionMeasurement(new Translation2d(randX, randY), now - delay /* - 0.1 * Math.random() */);
+            kalmanFilter.updateWithVisionMeasurement(new Translation2d(randX, randY), now - delay);
             SmartDashboard.putNumber("sensor X measurement", randX);
         }