Update Java Simulation Examples (#913)

Removes apriltagExample and simposeest, replacing them with swervedriveposeestsim --------- Co-authored-by: Matt <matthew.morley.ca@gmail.com>
2026-06-19 00:41:41 +00:00 · 2023-10-05 05:57:38 -07:00
parent 65d5494ab3
commit ce0d28da93
65 changed files with 1859 additions and 2011 deletions
--- a/photonlib-java-examples/simaimandrange/.gitignore
+++ b/photonlib-java-examples/simaimandrange/.gitignore
@@ -160,3 +160,4 @@ bin/

 # Simulation GUI and other tools window save file
 *-window.json
+networktables.json
--- a/photonlib-java-examples/simaimandrange/README.md
+++ b/photonlib-java-examples/simaimandrange/README.md
@@ -0,0 +1,3 @@
+## **`simaimandrange`**
+
+### See [PhotonLib Java Examples](./README.md#simaimandrange)
--- a/photonlib-java-examples/simaimandrange/simgui-ds.json
+++ b/photonlib-java-examples/simaimandrange/simgui-ds.json
@@ -1,28 +1,25 @@
 {
-  "Keyboard 0 Settings": {
-    "window": {
-      "visible": true
-    }
-  },
  "keyboardJoysticks": [
    {
      "axisConfig": [
+        {},
        {
          "decKey": 87,
          "incKey": 83
        },
-        {},
        {
-          "decKey": 69,
-          "decayRate": 0.0,
-          "incKey": 82,
-          "keyRate": 0.009999999776482582
+          "decayRate": 0.10000000149011612,
+          "incKey": 81,
+          "keyRate": 0.10000000149011612
        },
-        {},
-        {},
        {
-          "decKey": 68,
-          "incKey": 65
+          "decayRate": 0.10000000149011612,
+          "incKey": 69,
+          "keyRate": 0.10000000149011612
+        },
+        {
+          "decKey": 65,
+          "incKey": 68
        }
      ],
      "axisCount": 6,
--- a/photonlib-java-examples/simaimandrange/simgui.json
+++ b/photonlib-java-examples/simaimandrange/simgui.json
@@ -2,16 +2,47 @@
  "NTProvider": {
    "types": {
      "/FMSInfo": "FMSInfo",
-      "/SmartDashboard/Field": "Field2d",
-      "/SmartDashboard/photonvision Sim Field": "Field2d"
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
    },
    "windows": {
-      "/SmartDashboard/Field": {
-        "window": {
-          "visible": true
-        }
-      },
-      "/SmartDashboard/photonvision Sim Field": {
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "Robot": {
+          "arrowColor": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "arrowWeight": 3.0,
+          "color": [
+            1.0,
+            1.0,
+            1.0,
+            255.0
+          ],
+          "weight": 3.0
+        },
+        "cameras": {
+          "arrowColor": [
+            0.0,
+            0.683544397354126,
+            1.0,
+            255.0
+          ],
+          "arrowSize": 29,
+          "selectable": false,
+          "style": "Hidden"
+        },
+        "targets": {
+          "color": [
+            0.05063295364379883,
+            1.0,
+            0.0,
+            255.0
+          ],
+          "style": "Hidden",
+          "weight": 1.0
+        },
        "window": {
          "visible": true
        }
@@ -21,7 +52,7 @@
  "NetworkTables": {
    "transitory": {
      "SmartDashboard": {
-        "Field": {
+        "VisionSystemSim-main/Sim Field": {
          "open": true
        },
        "open": true
@@ -33,12 +64,5 @@
        }
      }
    }
-  },
-  "Plot": {
-    "Plot <0>": {
-      "window": {
-        "visible": false
-      }
-    }
  }
 }
--- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java
+++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java
@@ -56,8 +56,8 @@ public class Robot extends TimedRobot {
    PhotonCamera camera = new PhotonCamera("photonvision");

    // PID constants should be tuned per robot
-    final double LINEAR_P = 2.0;
-    final double LINEAR_D = 0.0;
+    final double LINEAR_P = 0.5;
+    final double LINEAR_D = 0.1;
    PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);

    final double ANGULAR_P = 0.03;
@@ -71,6 +71,12 @@ public class Robot extends TimedRobot {
    PWMVictorSPX rightMotor = new PWMVictorSPX(1);
    DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);

+    @Override
+    public void robotInit() {
+        leftMotor.setInverted(false);
+        rightMotor.setInverted(true);
+    }
+
    @Override
    public void teleopPeriodic() {
        double forwardSpeed;
@@ -91,11 +97,11 @@ public class Robot extends TimedRobot {
                                Units.degreesToRadians(result.getBestTarget().getPitch()));

                // Use this range as the measurement we give to the PID controller.
-                // -1.0 required to ensure positive PID controller effort _increases_ range
+                // (This forwardSpeed must be positive to go forward.)
                forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS);

                // Also calculate angular power
-                // -1.0 required to ensure positive PID controller effort _increases_ yaw
+                // (This rotationSpeed must be positive to turn counter-clockwise.)
                rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0);
            } else {
                // If we have no targets, stay still.
@@ -104,8 +110,8 @@ public class Robot extends TimedRobot {
            }
        } else {
            // Manual Driver Mode
-            forwardSpeed = -xboxController.getRightY();
-            rotationSpeed = xboxController.getLeftX();
+            forwardSpeed = -xboxController.getLeftY() * 0.75;
+            rotationSpeed = -xboxController.getRightX() * 0.75;
        }

        // Use our forward/turn speeds to control the drivetrain
@@ -119,7 +125,7 @@ public class Robot extends TimedRobot {

    @Override
    public void simulationInit() {
-        dtSim = new DrivetrainSim();
+        dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera);
    }

    @Override
--- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java
+++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java
@@ -26,6 +26,7 @@ package frc.robot.sim;

 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -38,11 +39,14 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
 import edu.wpi.first.wpilibj.simulation.PWMSim;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
-import org.photonvision.simulation.SimVisionSystem;
-import org.photonvision.simulation.SimVisionTarget;
+import java.util.List;
+import org.photonvision.PhotonCamera;
+import org.photonvision.estimation.TargetModel;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.simulation.VisionTargetSim;

 /**
 * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
@@ -53,53 +57,48 @@ import org.photonvision.simulation.SimVisionTarget;
 */
 public class DrivetrainSim {
    // Simulated Motor Controllers
-    PWMSim leftLeader = new PWMSim(0);
-    PWMSim rightLeader = new PWMSim(1);
+    PWMSim leftLeader;
+    PWMSim rightLeader;

    // Simulation Physics
    // Configure these to match your drivetrain's physical dimensions
    // and characterization results.
+    double trackwidthMeters = Units.feetToMeters(2.0);
    LinearSystem<N2, N2, N2> drivetrainSystem =
-            LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3, 1.0);
+            LinearSystemId.identifyDrivetrainSystem(2.0, 0.5, 2.25, 0.3, trackwidthMeters);
    DifferentialDrivetrainSim drivetrainSimulator =
            new DifferentialDrivetrainSim(
                    drivetrainSystem,
                    DCMotor.getCIM(2),
                    8,
-                    Units.feetToMeters(2.0),
+                    trackwidthMeters,
                    Units.inchesToMeters(6.0 / 2.0),
                    null);

    // Simulated Vision System.
    // Configure these to match your PhotonVision Camera,
    // pipeline, and LED setup.
-    double camDiagFOV = 170.0; // degrees - assume wide-angle camera
+    double camDiagFOV = 100.0; // degrees
    double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees
    double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
+    double minTargetArea = 0.1; // percentage (0 - 100)
    double maxLEDRange = 20; // meters
    int camResolutionWidth = 640; // pixels
    int camResolutionHeight = 480; // pixels
-    double minTargetArea = 10; // square pixels
+    PhotonCameraSim cameraSim;

-    SimVisionSystem simVision =
-            new SimVisionSystem(
-                    "photonvision",
-                    camDiagFOV,
-                    new Transform3d(
-                            new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)),
-                    maxLEDRange,
-                    camResolutionWidth,
-                    camResolutionHeight,
-                    minTargetArea);
+    VisionSystemSim visionSim = new VisionSystemSim("main");

    // See
    // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
    // page 208
-    double targetWidth = Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters
-    // See
-    // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
-    // page 197
-    double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
+    TargetModel targetModel =
+            new TargetModel(
+                    List.of(
+                            new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)),
+                            new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)),
+                            new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)),
+                            new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5))));
    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
    // pages 4 and 5
    double tgtXPos = Units.feetToMeters(54);
@@ -108,13 +107,36 @@ public class DrivetrainSim {
    Pose3d farTargetPose =
            new Pose3d(
                    new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
-                    new Rotation3d(0.0, 0.0, 0.0));
+                    new Rotation3d(0.0, 0.0, Math.PI));

-    Field2d field = new Field2d();
+    public DrivetrainSim(int leftMotorPort, int rightMotorPort, PhotonCamera camera) {
+        leftLeader = new PWMSim(leftMotorPort);
+        rightLeader = new PWMSim(rightMotorPort);

-    public DrivetrainSim() {
-        simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1));
-        SmartDashboard.putData("Field", field);
+        // Make the vision target visible to this simulated field.
+        var visionTarget = new VisionTargetSim(farTargetPose, targetModel);
+        visionSim.addVisionTargets(visionTarget);
+
+        // Create simulated camera properties. These can be set to mimic your actual camera.
+        var cameraProp = new SimCameraProperties();
+        cameraProp.setCalibration(
+                camResolutionWidth, camResolutionHeight, Rotation2d.fromDegrees(camDiagFOV));
+        cameraProp.setCalibError(0.2, 0.05);
+        cameraProp.setFPS(25);
+        cameraProp.setAvgLatencyMs(30);
+        cameraProp.setLatencyStdDevMs(4);
+        // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible
+        // targets.
+        cameraSim = new PhotonCameraSim(camera, cameraProp, minTargetArea, maxLEDRange);
+
+        // Add the simulated camera to view the targets on this simulated field.
+        visionSim.addCamera(
+                cameraSim,
+                new Transform3d(
+                        new Translation3d(0.25, 0, Robot.CAMERA_HEIGHT_METERS),
+                        new Rotation3d(0, -Robot.CAMERA_PITCH_RADIANS, 0)));
+
+        cameraSim.enableDrawWireframe(true);
    }

    /**
@@ -131,14 +153,12 @@ public class DrivetrainSim {
        }

        drivetrainSimulator.setInputs(
-                leftMotorCmd * RobotController.getInputVoltage(),
-                -rightMotorCmd * RobotController.getInputVoltage());
+                leftMotorCmd * RobotController.getBatteryVoltage(),
+                -rightMotorCmd * RobotController.getBatteryVoltage());
        drivetrainSimulator.update(0.02);

        // Update PhotonVision based on our new robot position.
-        simVision.processFrame(drivetrainSimulator.getPose());
-
-        field.setRobotPose(drivetrainSimulator.getPose());
+        visionSim.update(drivetrainSimulator.getPose());
    }

    /**
@@ -149,5 +169,6 @@ public class DrivetrainSim {
     */
    public void resetPose(Pose2d pose) {
        drivetrainSimulator.setPose(pose);
+        visionSim.resetRobotPose(pose);
    }
 }