yoinked 5026 code and tried to fix pose est

.github/workflows/build-test.yml

@@ -32,7 +32,13 @@ jobs:
     # Grant execute permission for gradlew
     - name: Grant execute permission for gradlew
       run: chmod +x gradlew
-
+    # Bump version
+    - name: Set up JDK 17
+        uses: actions/setup-java@v3
+        with:
+          distribution: temurin
+          java-version: 17
+          cache: gradle
     # Runs a single command using the runners shell
     - name: Compile and run tests on robot code
       run: ./gradlew build

build.gradle

@@ -4,8 +4,8 @@ plugins {
     id 'com.diffplug.spotless' version '6.12.0'
 }
 
-sourceCompatibility = JavaVersion.VERSION_11
-targetCompatibility = JavaVersion.VERSION_11
+sourceCompatibility = JavaVersion.VERSION_17
+targetCompatibility = JavaVersion.VERSION_17
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 

src/main/deploy/pathplanner/pose test Blue.path

@@ -0,0 +1,174 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 2.3,
+        "y": 1.8
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3000000000000003,
+        "y": 1.8
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.737025208647575,
+        "y": 1.8
+      },
+      "prevControl": {
+        "x": 3.737025208647575,
+        "y": 1.8
+      },
+      "nextControl": {
+        "x": 3.737025208647575,
+        "y": 1.8
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.5,
+        "y": 1.8
+      },
+      "prevControl": {
+        "x": 2.014223686941068,
+        "y": 1.8
+      },
+      "nextControl": {
+        "x": 2.985776313058932,
+        "y": 1.8
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.5,
+        "y": 1.8046074006839954
+      },
+      "prevControl": {
+        "x": 4.27385240613827,
+        "y": 1.8046074006839954
+      },
+      "nextControl": {
+        "x": 4.72614759386173,
+        "y": 1.8046074006839954
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.5,
+        "y": 1.8
+      },
+      "prevControl": {
+        "x": 2.2428113772359337,
+        "y": 1.8
+      },
+      "nextControl": {
+        "x": 2.7571886227640663,
+        "y": 1.8
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.5,
+        "y": 1.8
+      },
+      "prevControl": {
+        "x": 4.380109782805397,
+        "y": 1.8
+      },
+      "nextControl": {
+        "x": 4.619890217194603,
+        "y": 1.8
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.3,
+        "y": 1.8
+      },
+      "prevControl": {
+        "x": 1.2999999999999998,
+        "y": 1.8
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/java/frc/robot/Constants.java

@@ -36,12 +36,14 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.lib.util.COTSFalconSwerveConstants;
 import frc.lib.util.SwerveModuleConstants;
+import frc.robot.subsystems.Vision.VisionHelper;
+import frc.robot.subsystems.Vision.VisionHelper.TagCountDeviation;
+import frc.robot.subsystems.Vision.VisionHelper.UnitDeviationParams;
+
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
-import java.util.Optional;
 import java.util.Set;
-import org.photonvision.PhotonCamera;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -571,10 +573,27 @@ public static final class PoseEstimator {
                 1, // y
                 1 * Math.PI // theta
                 );
-
+
+                public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS =
+                List.of(
+                    // 1 tag
+                    new TagCountDeviation(
+                        new UnitDeviationParams(.25, .4, .9),
+                        new UnitDeviationParams(.35, .5, 1.2),
+                        new UnitDeviationParams(.5, .7, 1.5)),
+
+                    // 2 tags
+                    new TagCountDeviation(
+                        new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)),
+
+                    // 3+ tags
+                    new TagCountDeviation(
+                        new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5)));
+
     /** The distance at which tag distance is factored into deviation */
     public static final double NOISY_DISTANCE_METERS = 1.5;
 
+    public static final double POSE_AMBIGUITY_CUTOFF = .05;
     /**
      * The number to multiply by the smallest of the distance minus the above constant, clamped
      * above 1 to be the numerator of the fraction.

src/main/java/frc/robot/commands/PathplannerAutoChooser.java

@@ -149,6 +149,7 @@ public PathplannerAutoChooser(
     chooser.addOption("2 Piece Middle", () -> twoPieceMiddle());
     chooser.addOption("3 Piece Bottom", () -> threeBottom());
     chooser.addOption("Better 2 Bottom", () -> twoPieceBumpBetter());
+    chooser.addOption("pose test", () -> poseTest());
     chooser.addOption(
         "just score",
         () ->
@@ -214,6 +215,9 @@ public Command twoPieceBumpBetter() {
   public Command twoAndAHalfTop() {
     return auto("2.5 Piece Top");
   }
+  public Command poseTest() {
+    return auto("pose test");
+  }
 
   private static Command run(Command... commands) {
     return new ParallelCommandGroup(commands);

src/main/java/frc/robot/subsystems/Swerve/SwerveSubsystem.java

@@ -79,7 +79,6 @@ public class SwerveSubsystem extends SubsystemBase {
   public GyroIO gyroIO;
   public GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
   public double simHeading = 0.0;
-  public VisionHelper loggedEstimator;
 
   public Field2d field = new Field2d();
 
@@ -113,6 +112,10 @@ public void updateInputs(VisionIOInputs inputs, Pose3d robotPose) {}
   private ArrayList<Pose2d> dashboardFieldVisionPoses = new ArrayList<>();
   private ArrayList<Pose2d> dashboardFieldTapePoses = new ArrayList<>();
 
+  //Not actually used but the estimator constructor wants it
+  Vector<N3> odoStdDevs = VecBuilder.fill(0.3, 0.3, 0.01);
+  Vector<N3> visStdDevs = VecBuilder.fill(1.3, 1.3, 3.3);
+
   public ProfiledPIDController headingController =
       new ProfiledPIDController(1.2, 0, 0.1, new Constraints(Math.PI * 4, Math.PI * 6));
 
@@ -142,9 +145,6 @@ public SwerveSubsystem(SwerveModuleIO[] swerveIO, GyroIO gyroIO) {
     Timer.delay(1.0);
     resetModulesToAbsolute();
 
-    Vector<N3> odoStdDevs = VecBuilder.fill(0.3, 0.3, 0.01);
-    Vector<N3> visStdDevs = VecBuilder.fill(1.3, 1.3, 3.3);
-
     poseEstimator =
         new SwerveDrivePoseEstimator(
             Constants.Swerve.swerveKinematics,
@@ -652,13 +652,16 @@ public void periodic() {
     result.setTimestampSeconds(visionIOInputs.timestamp);
 
     try {
+      var estPose = VisionHelper.update(
+        result, tagFieldLayout, PoseStrategy.MULTI_TAG_PNP, PoseStrategy.LOWEST_AMBIGUITY).get();
       var visionMeasurement =
           VisionHelper.update(
                   result, tagFieldLayout, PoseStrategy.MULTI_TAG_PNP, PoseStrategy.LOWEST_AMBIGUITY)
               .get()
               .estimatedPose;
       Logger.getInstance().recordOutput("Vision Pose", visionMeasurement);
-      poseEstimator.addVisionMeasurement(visionMeasurement.toPose2d(), visionIOInputs.timestamp);
+        poseEstimator.addVisionMeasurement(visionMeasurement.toPose2d(), visionIOInputs.timestamp, VisionHelper.findVisionMeasurements(estPose));
+        resetOdometry(visionMeasurement.toPose2d());
     } catch (NoSuchElementException e) {}
 
     double[] apriltagX = new double[visionIOInputs.targets.size() * 4];