Merge pull request #19 from BroncBotz3481/dev

Fixed up testing.
BroncBotz3481 · Sep 14, 2024 · 6d28bc4 · 6d28bc4
2 parents 00b1682 + 9b93ba4
commit 6d28bc4
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 44 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -68,7 +68,7 @@ public class SwerveSubsystem extends SubsystemBase
   /**
    * Enable vision odometry updates while driving.
    */
-  private final boolean visionDriveTest = true;
+  private final boolean visionDriveTest = false;
 
   /**
    * Initialize {@link SwerveDrive} with the directory provided.
@@ -107,6 +107,8 @@ public SwerveSubsystem(File directory)
     if (visionDriveTest)
     {
       setupPhotonVision();
+      // Stop the odometry thread if we are using vision that way we can synchronize updates better.
+      swerveDrive.stopOdometryThread();
     }
     setupPathPlanner();
   }
@@ -128,26 +130,22 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
   public void setupPhotonVision()
   {
     vision = new Vision(swerveDrive::getPose, swerveDrive.field);
-    vision.updatePoseEstimation(swerveDrive);
   }
 
-  /**
-   * Update the pose estimation with vision data.
-   */
-  public void updatePoseWithVision()
+  @Override
+  public void periodic()
   {
-    vision.updatePoseEstimation(swerveDrive);
+    // When vision is enabled we must manually update odometry in SwerveDrive
+    if (visionDriveTest)
+    {
+      swerveDrive.updateOdometry();
+      vision.updatePoseEstimation(swerveDrive);
+    }
   }
 
-  /**
-   * Get the pose while updating with vision readings.
-   *
-   * @return The robots pose with the vision estimates in place.
-   */
-  public Pose2d getVisionPose()
+  @Override
+  public void simulationPeriodic()
   {
-    vision.updatePoseEstimation(swerveDrive);
-    return swerveDrive.getPose();
   }
 
   /**
@@ -324,10 +322,6 @@ public Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier trans
   {
     // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control.
     return run(() -> {
-      if (visionDriveTest)
-      {
-        vision.updatePoseEstimation(swerveDrive);
-      }
       // Make the robot move
       driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(translationX.getAsDouble(),
                                                                       translationY.getAsDouble(),
@@ -472,15 +466,6 @@ public void drive(ChassisSpeeds velocity)
     swerveDrive.drive(velocity);
   }
 
-  @Override
-  public void periodic()
-  {
-  }
-
-  @Override
-  public void simulationPeriodic()
-  {
-  }
 
   /**
    * Get the swerve drive kinematics object.

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java
@@ -65,13 +65,15 @@ public class Vision
    */
   private             Supplier<Pose2d>    currentPose;
   /**
-   * Photon Vision camera properties simulation.
+   * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
    */
+  private final double maximumAmbiguity = 0.25;
   /**
    * Field from {@link swervelib.SwerveDrive#field}
    */
   private             Field2d             field2d;
 
+
   /**
    * Constructor for the Vision class.
    *
@@ -155,11 +157,12 @@ public void updatePoseEstimation(SwerveDrive swerveDrive)
    */
   public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
   {
-    // Alternative method if you want to use both a pose filter and standard deviations based on distance + tags seen.
-    // Optional<EstimatedRobotPose> poseEst = filterPose(camera.poseEstimator.update());
-    Optional<EstimatedRobotPose> poseEst = camera.poseEstimator.update();
-    poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose")
-                                                   .setPose(estimatedRobotPose.estimatedPose.toPose2d()));
+    Optional<EstimatedRobotPose> poseEst = filterPose(camera.poseEstimator.update());
+    // Uncomment to enable outputting of vision targets in sim.
+    /*
+     poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose")
+                                                    .setPose(estimatedRobotPose.estimatedPose.toPose2d()));
+    */
     return poseEst;
   }
 
@@ -233,7 +236,7 @@ private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pos
         }
       }
       //ambiguity to high dont use estimate
-      if (bestTargetAmbiguity > 0.3)
+      if (bestTargetAmbiguity > maximumAmbiguity)
       {
         return Optional.empty();
       }
@@ -322,15 +325,15 @@ private void openSimCameraViews()
   {
     if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
     {
-      try
-      {
-        Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
-        Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
-        Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
-      } catch (IOException | URISyntaxException e)
-      {
-        e.printStackTrace();
-      }
+//      try
+//      {
+//        Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
+//        Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
+//        Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
+//      } catch (IOException | URISyntaxException e)
+//      {
+//        e.printStackTrace();
+//      }
     }
   }
 
@@ -475,6 +478,7 @@ public void addToVisionSim(VisionSystemSim systemSim)
       if (Robot.isSimulation())
       {
         systemSim.addCamera(cameraSim, robotToCamTransform);
+//        cameraSim.enableDrawWireframe(true);
       }
     }
   }