Complete simulation implementation

build.gradle

@@ -64,7 +64,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava
 wpi.java.debugJni = false
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.

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

@@ -18,7 +18,6 @@
 package frc.robot;
 
 import static edu.wpi.first.units.Units.*;
-import static frc.robot.util.RBSIEnum.*;
 
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
@@ -38,7 +37,15 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.SwerveConstants;
 import frc.robot.util.Alert;
+import frc.robot.util.RBSIEnum.AutoType;
+import frc.robot.util.RBSIEnum.CTREPro;
+import frc.robot.util.RBSIEnum.DriveStyle;
+import frc.robot.util.RBSIEnum.Mode;
+import frc.robot.util.RBSIEnum.MotorIdleMode;
+import frc.robot.util.RBSIEnum.SwerveType;
+import frc.robot.util.RBSIEnum.VisionType;
 import frc.robot.util.RobotDeviceId;
+import org.photonvision.simulation.SimCameraProperties;
 import swervelib.math.Matter;
 
 /**
@@ -64,7 +71,7 @@ public final class Constants {
   //       under strict caveat emptor -- and submit any error and bugfixes
   //       via GitHub issues.
   private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL
-  private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED
+  private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED
   private static AutoType autoType = AutoType.MANUAL; // MANUAL, PATHPLANNER, CHOREO
   private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE
 
@@ -102,7 +109,7 @@ public static void disableHAL() {
   /** Physical Constants for Robot Operation ******************************* */
   public static final class RobotConstants {
 
-    public static final Mass kRobotMass = Kilograms.of(100.);
+    public static final Mass kRobotMass = Pounds.of(100.);
     public static final Matter kChassis =
         new Matter(new Translation3d(0, 0, Inches.of(8).in(Meters)), kRobotMass.in(Kilograms));
     // Robot moment of intertial; this can be obtained from a CAD model of your drivetrain. Usually,
@@ -112,6 +119,10 @@ public static final class RobotConstants {
     // Wheel coefficient of friction
     public static final double kWheelCOF = 1.2;
 
+    // Maximum torque applied by wheel
+    // Kraken X60 stall torque ~7.09 Nm; MK4i L3 gear ratio 6.12:1
+    public static final double kMaxWheelTorque = 43.4; // Nm
+
     // Insert here the orientation (CCW == +) of the Rio and IMU from the robot
     // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference
     //   frame.
@@ -421,10 +432,14 @@ public static class Cameras {
 
     // Robot to camera transforms
     // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead)
+    // Example Camera are mounted on the frame perimeter, 18" up from the floor, centered
+    // side-to-side
     public static Transform3d robotToCamera0 =
-        new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
+        new Transform3d(
+            Inches.of(13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, 0.0));
     public static Transform3d robotToCamera1 =
-        new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
+        new Transform3d(
+            Inches.of(-13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, Math.PI));
 
     // Standard deviation multipliers for each camera
     // (Adjust to trust some cameras more than others)
@@ -435,6 +450,32 @@ public static class Cameras {
         };
   }
 
+  /************************************************************************* */
+  /** Simulation Camera Properties ***************************************** */
+  public static class SimCameras {
+    public static final SimCameraProperties kSimCamera1Props =
+        new SimCameraProperties() {
+          {
+            setCalibration(1280, 800, Rotation2d.fromDegrees(90));
+            setCalibError(0.25, 0.08);
+            setFPS(30);
+            setAvgLatencyMs(20);
+            setLatencyStdDevMs(5);
+          }
+        };
+
+    public static final SimCameraProperties kSimCamera2Props =
+        new SimCameraProperties() {
+          {
+            setCalibration(1280, 800, Rotation2d.fromDegrees(90));
+            setCalibError(0.25, 0.08);
+            setFPS(30);
+            setAvgLatencyMs(20);
+            setLatencyStdDevMs(5);
+          }
+        };
+  }
+
   /************************************************************************* */
   /** Deploy Directoy Location Constants *********************************** */
   public static final class DeployConstants {

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

@@ -18,6 +18,9 @@
 package frc.robot;
 
 import com.revrobotics.util.StatusLogger;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj.Timer;
@@ -31,6 +34,10 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import org.littletonrobotics.urcl.URCL;
+import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -43,6 +50,9 @@ public class Robot extends LoggedRobot {
   private RobotContainer m_robotContainer;
   private Timer m_disabledTimer;
 
+  // Define simulation fields here
+  private VisionSystemSim visionSim;
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -108,7 +118,9 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     // Switch thread to high priority to improve loop timing
-    Threads.setCurrentThreadPriority(true, 99);
+    if (isReal()) {
+      Threads.setCurrentThreadPriority(true, 99);
+    }
 
     // Run all virtual subsystems each time through the loop
     VirtualSubsystem.periodicAll();
@@ -202,7 +214,7 @@ public void teleopPeriodic() {
 
     // For 2026 - REBUILT, the alliance will be provided as a single character
     //   representing the color of the alliance whose goal will go inactive
-    //   first (i.e. ‘R’ = red, ‘B’ = blue). This alliance’s goal will be
+    //   first (i.e. 'R' = red, 'B' = blue). This alliance's goal will be
     //   active in Shifts 2 and 4.
     //
     // https://docs.wpilib.org/en/stable/docs/yearly-overview/2026-game-data.html
@@ -242,9 +254,38 @@ public void testPeriodic() {}
 
   /** This function is called once when the robot is first started up. */
   @Override
-  public void simulationInit() {}
+  public void simulationInit() {
+    visionSim = new VisionSystemSim("main");
+
+    // Load AprilTag field layout
+    AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+    // Register AprilTags with vision simulation
+    visionSim.addAprilTags(fieldLayout);
+
+    // Simulated Camera Properties
+    SimCameraProperties cameraProp = new SimCameraProperties();
+    // A 1280 x 800 camera with a 100 degree diagonal FOV.
+    cameraProp.setCalibration(1280, 800, Rotation2d.fromDegrees(100));
+    // Approximate detection noise with average and standard deviation error in pixels.
+    cameraProp.setCalibError(0.25, 0.08);
+    // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+    cameraProp.setFPS(20);
+    // The average and standard deviation in milliseconds of image data latency.
+    cameraProp.setAvgLatencyMs(35);
+    cameraProp.setLatencyStdDevMs(5);
+
+    // Define Cameras and add to simulation
+    PhotonCamera camera0 = new PhotonCamera("frontCam");
+    PhotonCamera camera1 = new PhotonCamera("backCam");
+    visionSim.addCamera(new PhotonCameraSim(camera0, cameraProp), Constants.Cameras.robotToCamera0);
+    visionSim.addCamera(new PhotonCameraSim(camera1, cameraProp), Constants.Cameras.robotToCamera1);
+  }
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    // Update sim each sim tick
+    visionSim.update(m_robotContainer.getDrivebase().getPose());
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -23,17 +23,21 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.OperatorConstants;
+import frc.robot.Constants.SimCameras;
 import frc.robot.FieldConstants.AprilTagLayoutType;
 import frc.robot.commands.AutopilotCommands;
 import frc.robot.commands.DriveCommands;
@@ -47,6 +51,7 @@
 import frc.robot.subsystems.imu.ImuIONavX;
 import frc.robot.subsystems.imu.ImuIOPigeon2;
 import frc.robot.subsystems.imu.ImuIOSim;
+import frc.robot.subsystems.vision.CameraSweepEvaluator;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOLimelight;
@@ -61,6 +66,9 @@
 import frc.robot.util.RBSIPowerMonitor;
 import java.util.Set;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.VisionSystemSim;
 
 /** This is the location for defining robot hardware, commands, and controller button bindings. */
 public class RobotContainer {
@@ -72,6 +80,10 @@ public class RobotContainer {
   final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator
   final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches
 
+  // These two are needed for the Sweep evaluator for camera FOV simulation
+  final CommandJoystick joystick3 = new CommandJoystick(3); //  Joystick for CamersSweepEvaluator
+  private final CameraSweepEvaluator sweep;
+
   /** Declare the robot subsystems here ************************************ */
   // These are the "Active Subsystems" that the robot controls
   private final Drive m_drivebase;
@@ -152,11 +164,12 @@ public RobotContainer() {
               default -> null;
             };
         m_accel = new Accelerometer(m_imu);
+        sweep = null;
         break;
 
       case SIM:
         // Sim robot, instantiate physics sim IO implementations
-        m_imu = new ImuIOSim(Constants.loopPeriodSecs);
+        m_imu = new ImuIOSim();
         m_drivebase = new Drive(m_imu);
         m_flywheel = new Flywheel(new FlywheelIOSim() {});
         m_vision =
@@ -165,16 +178,38 @@ public RobotContainer() {
                 new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, m_drivebase::getPose),
                 new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, m_drivebase::getPose));
         m_accel = new Accelerometer(m_imu);
+
+        // CameraSweepEvaluator Construct
+        // 1) Create the vision simulation world
+        VisionSystemSim visionSim = new VisionSystemSim("CameraSweepWorld");
+        // 2) Add AprilTags (field layout)
+        visionSim.addAprilTags(FieldConstants.aprilTagLayout);
+        // 3) Create two simulated cameras
+        // Create PhotonCamera objects (names must match VisionIOPhotonVisionSim)
+        PhotonCamera camera1 = new PhotonCamera(camera0Name);
+        PhotonCamera camera2 = new PhotonCamera(camera1Name);
+
+        // Wrap them with simulation + properties (2026 API)
+        PhotonCameraSim cam1 = new PhotonCameraSim(camera1, SimCameras.kSimCamera1Props);
+        PhotonCameraSim cam2 = new PhotonCameraSim(camera2, SimCameras.kSimCamera2Props);
+
+        // 4) Register cameras with the sim
+        visionSim.addCamera(cam1, Transform3d.kZero);
+        visionSim.addCamera(cam2, Transform3d.kZero);
+        // 5) Create the sweep evaluator
+        sweep = new CameraSweepEvaluator(visionSim, cam1, cam2);
+
         break;
 
       default:
         // Replayed robot, disable IO implementations
-        m_imu = new ImuIOSim(Constants.loopPeriodSecs);
+        m_imu = new ImuIOSim();
         m_drivebase = new Drive(m_imu);
         m_flywheel = new Flywheel(new FlywheelIO() {});
         m_vision =
             new Vision(m_drivebase::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
         m_accel = new Accelerometer(m_imu);
+        sweep = null;
         break;
     }
 
@@ -343,6 +378,25 @@ private void configureBindings() {
                 // Stop when command ended
                 m_drivebase::stop,
                 m_drivebase));
+
+    // Double-press the A button on Joystick3 to run the CameraSweepEvaluator
+    // Use WPILib's built-in double-press binding
+    joystick3
+        .button(1)
+        .multiPress(2, 0.2)
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  try {
+                    sweep.runFullSweep(
+                        Filesystem.getOperatingDirectory()
+                            .toPath()
+                            .resolve("camera_sweep.csv")
+                            .toString());
+                  } catch (Exception e) {
+                    e.printStackTrace();
+                  }
+                }));
   }
 
   /**
@@ -402,6 +456,11 @@ public void updateAlerts() {
     }
   }
 
+  /** Drivetrain getter method */
+  public Drive getDrivebase() {
+    return m_drivebase;
+  }
+
   /**
    * Set up the SysID routines from AdvantageKit
    *