[examples] Re-organize simaimandrange (#937)

photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java

@@ -0,0 +1,91 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+    // ---------- Vision
+    // 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
+    public static final double GOAL_RANGE_METERS = Units.feetToMeters(10);
+
+    // Where the 2020 High goal target is located on the field
+    // See
+    // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system
+    // and https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
+    // (pages 4 and 5)
+    public static final Pose3d TARGET_POSE =
+            new Pose3d(
+                    new Translation3d(
+                            Units.feetToMeters(52.46),
+                            Units.inchesToMeters(94.66),
+                            Units.inchesToMeters(89.69)), // (center of vision target)
+                    new Rotation3d(0.0, 0.0, Math.PI));
+    // ----------
+
+    // ---------- Drivetrain
+    public static final int LEFT_MOTOR_CHANNEL = 0;
+    public static final int RIGHT_MOTOR_CHANNEL = 1;
+
+    // PID constants should be tuned per robot
+    public static final double LINEAR_P = 0.5;
+    public static final double LINEAR_I = 0;
+    public static final double LINEAR_D = 0.1;
+
+    public static final double ANGULAR_P = 0.03;
+    public static final double ANGULAR_I = 0;
+    public static final double ANGULAR_D = 0.003;
+
+    // Ratio to multiply joystick inputs by
+    public static final double DRIVESPEED = 0.75;
+
+    // The following properties are necessary for simulation:
+
+    // Distance from drivetrain left wheels to right wheels
+    public static final double TRACKWIDTH_METERS = Units.feetToMeters(2.0);
+    public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(6.0);
+
+    // The motors used in the gearbox for one drivetrain side
+    public static final DCMotor DRIVE_MOTORS = DCMotor.getCIM(2);
+    // The gearbox reduction, or how many motor rotations per wheel rotation
+    public static final double GEARING = 8.0;
+
+    // The drivetrain feedforward values
+    public static final double LINEAR_KV = 2.0;
+    public static final double LINEAR_KA = 0.5;
+
+    public static final double ANGULAR_KV = 2.25;
+    public static final double ANGULAR_KA = 0.3;
+    // ----------
+}

photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java

@@ -24,13 +24,16 @@
 
 package frc.robot;
 
+import static frc.robot.Constants.*;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
 import frc.robot.sim.DrivetrainSim;
+import frc.robot.sim.VisionSim;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonUtils;
 
@@ -41,34 +44,18 @@
  * 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 = 0.5;
-    final double LINEAR_D = 0.1;
     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);
+    PWMVictorSPX leftMotor = new PWMVictorSPX(LEFT_MOTOR_CHANNEL);
+    PWMVictorSPX rightMotor = new PWMVictorSPX(RIGHT_MOTOR_CHANNEL);
     DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
 
     @Override
@@ -92,7 +79,7 @@ public void teleopPeriodic() {
                 double range =
                         PhotonUtils.calculateDistanceToTargetMeters(
                                 CAMERA_HEIGHT_METERS,
-                                TARGET_HEIGHT_METERS,
+                                TARGET_POSE.getZ(),
                                 CAMERA_PITCH_RADIANS,
                                 Units.degreesToRadians(result.getBestTarget().getPitch()));
 
@@ -110,8 +97,8 @@ public void teleopPeriodic() {
             }
         } else {
             // Manual Driver Mode
-            forwardSpeed = -xboxController.getLeftY() * 0.75;
-            rotationSpeed = -xboxController.getRightX() * 0.75;
+            forwardSpeed = -xboxController.getLeftY() * DRIVESPEED;
+            rotationSpeed = -xboxController.getRightX() * DRIVESPEED;
         }
 
         // Use our forward/turn speeds to control the drivetrain
@@ -122,14 +109,17 @@ public void teleopPeriodic() {
     // Simulation support
 
     DrivetrainSim dtSim;
+    VisionSim visionSim;
 
     @Override
     public void simulationInit() {
-        dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera);
+        dtSim = new DrivetrainSim(leftMotor, rightMotor);
+        visionSim = new VisionSim("main", camera);
     }
 
     @Override
     public void simulationPeriodic() {
         dtSim.update();
+        visionSim.update(dtSim.getPose());
     }
 }

photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java

@@ -24,119 +24,49 @@
 
 package frc.robot.sim;
 
+import static frc.robot.Constants.*;
+
 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;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
-import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
 import edu.wpi.first.wpilibj.simulation.PWMSim;
 import frc.robot.Robot;
-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
- * PhotonVision system and one vision target.
+ * This class handles the simulation of robot physics, sensors, and motor controllers.
  *
  * <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;
-    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(2.0, 0.5, 2.25, 0.3, trackwidthMeters);
-    DifferentialDrivetrainSim drivetrainSimulator =
+    // ----- Simulation specific constants
+    // Drivetrain plant and simulation. This will calculate how the robot pose changes over time as
+    // motor voltages are applied.
+    private static final LinearSystem<N2, N2, N2> drivetrainSystem =
+            LinearSystemId.identifyDrivetrainSystem(
+                    LINEAR_KV, LINEAR_KA, ANGULAR_KV, ANGULAR_KA, TRACKWIDTH_METERS);
+    private static final DifferentialDrivetrainSim drivetrainSimulator =
             new DifferentialDrivetrainSim(
                     drivetrainSystem,
-                    DCMotor.getCIM(2),
-                    8,
-                    trackwidthMeters,
-                    Units.inchesToMeters(6.0 / 2.0),
+                    DRIVE_MOTORS,
+                    GEARING,
+                    TRACKWIDTH_METERS,
+                    WHEEL_DIAMETER_METERS / 2.0,
                     null);
+    // -----
 
-    // Simulated Vision System.
-    // Configure these to match your PhotonVision Camera,
-    // pipeline, and LED setup.
-    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
-    PhotonCameraSim cameraSim;
-
-    VisionSystemSim visionSim = new VisionSystemSim("main");
-
-    // See
-    // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
-    // page 208
-    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);
-    double tgtYPos =
-            Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
-    Pose3d farTargetPose =
-            new Pose3d(
-                    new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
-                    new Rotation3d(0.0, 0.0, Math.PI));
-
-    public DrivetrainSim(int leftMotorPort, int rightMotorPort, PhotonCamera camera) {
-        leftLeader = new PWMSim(leftMotorPort);
-        rightLeader = new PWMSim(rightMotorPort);
+    // PWM handles for getting commanded motor controller speeds
+    private final PWMSim leftLeader;
+    private final PWMSim rightLeader;
 
-        // 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);
+    public DrivetrainSim(PWMMotorController leftController, PWMMotorController rightController) {
+        leftLeader = new PWMSim(leftController);
+        rightLeader = new PWMSim(rightController);
     }
 
     /**
@@ -155,20 +85,21 @@ public void update() {
         drivetrainSimulator.setInputs(
                 leftMotorCmd * RobotController.getBatteryVoltage(),
                 -rightMotorCmd * RobotController.getBatteryVoltage());
-        drivetrainSimulator.update(0.02);
 
-        // Update PhotonVision based on our new robot position.
-        visionSim.update(drivetrainSimulator.getPose());
+        drivetrainSimulator.update(Robot.kDefaultPeriod);
+    }
+
+    public Pose2d getPose() {
+        return 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).
+     * 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 (e.g. at the start of each match).
      *
      * @param pose
      */
     public void resetPose(Pose2d pose) {
         drivetrainSimulator.setPose(pose);
-        visionSim.resetRobotPose(pose);
     }
 }