add vision

Author: sophiep29

src/main/java/frc/robot/Robot.java

@@ -5,28 +5,29 @@
 package frc.robot;
 
 import static frc.robot.RobotContainer.shooterState;
-import static frc.robot.config.VisionConfig.photonPoseEstimatorForward;
-import static frc.robot.config.VisionConfig.result;
 
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.FlywheelCommand;
 import frc.robot.config.TunerConstants;
-import frc.robot.config.VisionConfig;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import java.util.Optional;
-import org.photonvision.EstimatedRobotPose;
 
 @Logged
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
   private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  // last time we injected a synthetic vision measurement (seconds, FPGA time)
+  private double m_lastSimVisionTime = 0.0;
+  // whether we've injected a one-time offset vision measurement for testing
+  private boolean m_injectedOffset = false;
 
   private final RobotContainer m_robotContainer;
 
@@ -39,22 +40,19 @@ public Robot() {
 
   @Override
   public void robotPeriodic() {
-    CommandScheduler.getInstance().run();
-    Optional<EstimatedRobotPose> visionEst =
-        VisionConfig.photonPoseEstimatorForward.estimateCoprocMultiTagPose(result);
-    if (visionEst.isEmpty()) {
-      visionEst = VisionConfig.photonPoseEstimatorForward.estimateLowestAmbiguityPose(result);
-    } else {
-      drivetrain.addVisionMeasurement(
-          photonPoseEstimatorForward
-              .estimateAverageBestTargetsPose(result)
-              .get()
-              .estimatedPose
-              .toPose2d(),
-          visionEst.get().timestampSeconds);
-    }
     SmartDashboard.putString("shoot-state", shooterState);
     SmartDashboard.putString("shoot-on", FlywheelCommand.isOn);
+    try {
+      var pose = drivetrain.getState().Pose;
+      SmartDashboard.putNumber("OdometryX", pose.getX());
+      SmartDashboard.putNumber("OdometryY", pose.getY());
+      SmartDashboard.putNumber("OdometryRotDeg", pose.getRotation().getDegrees());
+      SmartDashboard.putNumber("SimVisionLastInject", m_lastSimVisionTime);
+      System.out.printf(
+          "ODOM X=%.3f Y=%.3f R=%.2f SimVision=%.3f\n",
+          pose.getX(), pose.getY(), pose.getRotation().getDegrees(), m_lastSimVisionTime);
+    } catch (Exception ignored) {
+    }
   }
 
   @Override
@@ -109,5 +107,25 @@ public void testExit() {}
   public void simulationInit() {}
 
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    double now = Timer.getFPGATimestamp();
+    if (now - m_lastSimVisionTime >= 0.1) {
+      m_lastSimVisionTime = now;
+      try {
+        var truePose = drivetrain.getState().Pose;
+        // After 3 seconds, inject a single offset measurement (+1m X) to observe correction
+        if (!m_injectedOffset && now > 3.0) {
+          Pose2d offsetPose =
+              new Pose2d(truePose.getX() + 1.0, truePose.getY(), truePose.getRotation());
+          drivetrain.addVisionMeasurement(offsetPose, now);
+          System.out.println("SIM INJECT: OFFSET +1.0m X at " + now);
+          m_injectedOffset = true;
+        } else {
+          drivetrain.addVisionMeasurement(truePose, now);
+        }
+      } catch (Exception e) {
+        DriverStation.reportError("Sim vision injection failed: " + e.getMessage(), false);
+      }
+    }
+  }
 }

src/main/java/frc/robot/config/VisionConfig.java

@@ -2,17 +2,25 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
-import java.util.Optional;
-import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 public class VisionConfig {
   public static PhotonPipelineResult result = new PhotonPipelineResult();
-  public static final String CAMERA_NAME = "forwardAprilTag"; // left
+  public static final String FRONT_CAMERA_NAME = "forwardAprilTag";
+  // Per-camera measurement standard deviations: [x (m), y (m), theta (rad)]
+  public static final Matrix<N3, N1> FRONT_VISION_STDDEVS =
+      VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(3.0));
+  public static final Matrix<N3, N1> REAR_VISION_STDDEVS =
+      VecBuilder.fill(0.10, 0.10, Units.degreesToRadians(5.0));
+  public static final String REAR_CAMERA_NAME = "rearAprilTag";
   public static final AprilTagFieldLayout LAYOUT =
       AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
   public static final PhotonPoseEstimator.PoseStrategy STRATEGY =
@@ -23,7 +31,10 @@ public class VisionConfig {
           Units.inchesToMeters(-11.55),
           Units.inchesToMeters(14.88),
           new Rotation3d(0.0, Units.degreesToRadians(16), 0.0));
-  Optional<EstimatedRobotPose> visionEst = Optional.empty();
-  public static PhotonPoseEstimator photonPoseEstimatorForward =
-      new PhotonPoseEstimator(LAYOUT, FORWARD_CAMERA_POSITION);
+  public static final Transform3d REAR_CAMERA_POSITION =
+      new Transform3d(
+          Units.inchesToMeters(0.0),
+          Units.inchesToMeters(0.0),
+          Units.inchesToMeters(0.0),
+          new Rotation3d(0.0, Units.degreesToRadians(0.0), 0.0));
 }

src/main/java/frc/robot/subsystems/Vision.java

@@ -3,8 +3,10 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.config.VisionConfig;
 import org.photonvision.PhotonCamera;
-import org.photonvision.estimation.*;
 
 public class Vision extends SubsystemBase {
-  public static final PhotonCamera FrontCameraApril = new PhotonCamera(VisionConfig.CAMERA_NAME);
+  public static final PhotonCamera FrontCameraApril =
+      new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME);
+  public static final PhotonCamera RearCameraApril =
+      new PhotonCamera(VisionConfig.REAR_CAMERA_NAME);
 }