Add Basic Sim Example (#237)

* WIP adding sim pose example * WIP making examples buildable like WPI. Not quite there yet.... * Make examples runnable * remove lock * add lock * WIP Adding a simpler example for simulation * Spotless Apply * Added simulation-supporting aim and range example * Spotless, revised hand usage to be consistent across examples, and propagated required -1.0's to non-sim examples. Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
2026-06-24 01:31:44 +00:00 · 2021-01-26 22:26:15 -06:00
parent 4c15a46cda
commit 0330467874
19 changed files with 708 additions and 29 deletions
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Robot.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Robot.java
@@ -82,10 +82,12 @@ public class Robot extends TimedRobot {
                                result.getBestTarget().getPitch());

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

                // Also calculate angular power
-                rotationSpeed = turnController.calculate(result.getBestTarget().getYaw(), 0);
+                // -1.0 required to ensure positive PID controller effort _increases_ yaw
+                rotationSpeed = -1.0 * turnController.calculate(result.getBestTarget().getYaw(), 0);
            } else {
                // If we have no targets, stay still.
                forwardSpeed = 0;
@@ -93,7 +95,7 @@ public class Robot extends TimedRobot {
            }
        } else {
            // Manual Driver Mode
-            forwardSpeed = xboxController.getY(GenericHID.Hand.kRight);
+            forwardSpeed = -1.0 * xboxController.getY(GenericHID.Hand.kRight);
            rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft);
        }

--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Robot.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Robot.java
@@ -25,7 +25,6 @@ import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.util.Units;
 import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonUtils;

 /**
 * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -67,33 +66,23 @@ public class Robot extends TimedRobot {
        double forwardSpeed;
        double rotationSpeed;

+        forwardSpeed = -1.0 * xboxController.getY(GenericHID.Hand.kRight);
+
        if (xboxController.getAButton()) {
            // Vision-alignment mode
            // Query the latest result from PhotonVision
            var result = camera.getLatestResult();

            if (result.hasTargets()) {
-                // First calculate range
-                double range =
-                        PhotonUtils.calculateDistanceToTargetMeters(
-                                CAMERA_HEIGHT_METERS,
-                                TARGET_HEIGHT_METERS,
-                                CAMERA_PITCH_RADIANS,
-                                result.getBestTarget().getPitch());
-
-                // Use this range as the measurement we give to the PID controller.
-                forwardSpeed = forwardController.calculate(range, GOAL_RANGE_METERS);
-
-                // Also calculate angular power
-                rotationSpeed = turnController.calculate(result.getBestTarget().getYaw(), 0);
+                // Calculate angular turn power
+                // -1.0 required to ensure positive PID controller effort _increases_ yaw
+                rotationSpeed = -1.0 * turnController.calculate(result.getBestTarget().getYaw(), 0);
            } else {
                // If we have no targets, stay still.
-                forwardSpeed = 0;
                rotationSpeed = 0;
            }
        } else {
            // Manual Driver Mode
-            forwardSpeed = xboxController.getY(GenericHID.Hand.kRight);
            rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft);
        }

--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json
@@ -35,6 +35,18 @@
    "dependencies": [],
    "foldername": "aimandrange"
  },
+  {
+    "name": "SimAimAndRange",
+    "description": "Adding Simulation Support to the Aim And Range example",
+    "tags": [],
+    "gradlebase": "java",
+    "language": "java",
+    "commandversion": 1,
+    "mainclass": "Main",
+    "packagetoreplace": null,
+    "dependencies": [],
+    "foldername": "simaimandrange"
+  },
  {
    "name": "SimPoseEstimation",
    "description": "Integrate 3D vision processing mode results into estimation of robot pose on the field. Includes simulation support.",
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java
@@ -84,14 +84,15 @@ public class Robot extends TimedRobot {
                                result.getBestTarget().getPitch());

                // Use this range as the measurement we give to the PID controller.
-                forwardSpeed = controller.calculate(range, GOAL_RANGE_METERS);
+                // -1.0 required to ensure positive PID controller effort _increases_ range
+                forwardSpeed = -1.0 * controller.calculate(range, GOAL_RANGE_METERS);
            } else {
                // If we have no targets, stay still.
                forwardSpeed = 0;
            }
        } else {
            // Manual Driver Mode
-            forwardSpeed = xboxController.getY(GenericHID.Hand.kRight);
+            forwardSpeed = -1.0 * xboxController.getY(GenericHID.Hand.kRight);
        }

        // Use our forward/turn speeds to control the drivetrain
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/Main.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/Main.java
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simaimandrange;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+* you are doing, do not modify this file except to change the parameter class to the startRobot
+* call.
+*/
+public final class Main {
+    private Main() {}
+
+    /**
+    * Main initialization function. Do not perform any initialization here.
+    *
+    * <p>If you change your main robot class, change the parameter type.
+    */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
+}
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/Robot.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/Robot.java
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simaimandrange;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.util.Units;
+import org.photonlib.examples.simaimandrange.sim.DrivetrainSim;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonUtils;
+
+/**
+* The VM is configured to automatically run this class, and to call the functions corresponding to
+* each mode, as described in the TimedRobot documentation. If you change the name of this class or
+* the package after creating this project, you must also update the build.gradle file in the
+* project.
+*/
+public class Robot extends TimedRobot {
+    // 2020 High goal target height above ground
+    public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
+
+    // Constants about how your camera is mounted to the robot
+    public static final double CAMERA_PITCH_RADIANS =
+            Units.degreesToRadians(15); // Angle "up" from horizontal
+    public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor
+
+    // How far from the target we want to be
+    final double GOAL_RANGE_METERS = Units.feetToMeters(10);
+
+    // Change this to match the name of your camera
+    PhotonCamera camera = new PhotonCamera("photonvision");
+
+    // PID constants should be tuned per robot
+    final double LINEAR_P = 2.0;
+    final double LINEAR_D = 0.0;
+    PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
+
+    final double ANGULAR_P = 0.03;
+    final double ANGULAR_D = 0.003;
+    PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
+
+    XboxController xboxController = new XboxController(0);
+
+    // Drive motors
+    PWMVictorSPX leftMotor = new PWMVictorSPX(0);
+    PWMVictorSPX rightMotor = new PWMVictorSPX(1);
+    DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
+
+    @Override
+    public void teleopPeriodic() {
+        double forwardSpeed;
+        double rotationSpeed;
+
+        if (xboxController.getAButton()) {
+            // Vision-alignment mode
+            // Query the latest result from PhotonVision
+            var result = camera.getLatestResult();
+
+            if (result.hasTargets()) {
+                // First calculate range
+                double range =
+                        PhotonUtils.calculateDistanceToTargetMeters(
+                                CAMERA_HEIGHT_METERS,
+                                TARGET_HEIGHT_METERS,
+                                CAMERA_PITCH_RADIANS,
+                                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
+                forwardSpeed = -1.0 * forwardController.calculate(range, GOAL_RANGE_METERS);
+
+                // Also calculate angular power
+                // -1.0 required to ensure positive PID controller effort _increases_ yaw
+                rotationSpeed = -1.0 * turnController.calculate(result.getBestTarget().getYaw(), 0);
+            } else {
+                // If we have no targets, stay still.
+                forwardSpeed = 0;
+                rotationSpeed = 0;
+            }
+        } else {
+            // Manual Driver Mode
+            forwardSpeed = -1.0 * xboxController.getY(GenericHID.Hand.kRight);
+            rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft);
+        }
+
+        // Use our forward/turn speeds to control the drivetrain
+        drive.arcadeDrive(forwardSpeed, rotationSpeed);
+    }
+
+    ////////////////////////////////////////////////////
+    // Simulation support
+
+    DrivetrainSim dtSim;
+
+    @Override
+    public void simulationInit() {
+        dtSim = new DrivetrainSim();
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        dtSim.update();
+    }
+}
--- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java
+++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java
@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simaimandrange.sim;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+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 edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import org.photonlib.examples.simaimandrange.Robot;
+import org.photonvision.SimVisionSystem;
+import org.photonvision.SimVisionTarget;
+
+/**
+* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
+* PhotonVision system and one vision target.
+*
+* <p>This class and its methods are only relevant during simulation. While on the real robot, the
+* real motors/sensors/physics are used instead.
+*/
+public class DrivetrainSim {
+
+    // Simulated Motor Controllers
+    PWMSim leftLeader = new PWMSim(0);
+    PWMSim rightLeader = new PWMSim(1);
+
+    // Simulation Physics
+    // Configure these to match your drivetrain's physical dimensions
+    // and characterization results.
+    LinearSystem<N2, N2, N2> drivetrainSystem =
+            LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+    DifferentialDrivetrainSim drivetrainSimulator =
+            new DifferentialDrivetrainSim(
+                    drivetrainSystem,
+                    DCMotor.getCIM(2),
+                    8,
+                    Units.feetToMeters(2.0),
+                    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 camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees
+    double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
+    double maxLEDRange = 20; // meters
+    int camResolutionWidth = 640; // pixels
+    int camResolutionHeight = 480; // pixels
+    double minTargetArea = 10; // square pixels
+
+    SimVisionSystem simVision =
+            new SimVisionSystem(
+                    "photonvision",
+                    camDiagFOV,
+                    camPitch,
+                    new Transform2d(),
+                    camHeightOffGround,
+                    maxLEDRange,
+                    camResolutionWidth,
+                    camResolutionHeight,
+                    minTargetArea);
+
+    // 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
+    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
+    // pages 4 and 5
+    double tgtXPos = Units.feetToMeters(54);
+    double tgtYPos =
+            Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
+    Pose2d farTargetPose = new Pose2d(new Translation2d(tgtXPos, tgtYPos), new Rotation2d(0.0));
+
+    Field2d field = new Field2d();
+
+    public DrivetrainSim() {
+        simVision.addSimVisionTarget(
+                new SimVisionTarget(farTargetPose, Robot.TARGET_HEIGHT_METERS, targetWidth, targetHeight));
+        SmartDashboard.putData("Field", field);
+    }
+
+    /**
+    * Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
+    * physics forward by a single 20ms step.
+    */
+    public void update() {
+
+        double leftMotorCmd = 0;
+        double rightMotorCmd = 0;
+
+        if (DriverStation.getInstance().isEnabled() && !RobotController.isBrownedOut()) {
+            leftMotorCmd = leftLeader.getSpeed();
+            rightMotorCmd = rightLeader.getSpeed();
+        }
+
+        drivetrainSimulator.setInputs(
+                leftMotorCmd * RobotController.getInputVoltage(),
+                -rightMotorCmd * RobotController.getInputVoltage());
+        drivetrainSimulator.update(0.02);
+
+        // Update PhotonVision based on our new robot position.
+        simVision.processFrame(drivetrainSimulator.getPose());
+
+        field.setRobotPose(drivetrainSimulator.getPose());
+    }
+
+    /**
+    * Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
+    * robot onto a specific spot in the field (IE, at the start of each match).
+    *
+    * @param pose
+    */
+    public void resetPose(Pose2d pose) {
+        drivetrainSimulator.setPose(pose);
+    }
+}