diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000000..8e0f41cd57 --- /dev/null +++ b/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; + // ---------- +} diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java index 53197dd216..4f22aa287c 100644 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java @@ -24,6 +24,8 @@ 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; @@ -31,6 +33,7 @@ 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()); } } diff --git 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 index 0034ac8772..2a9c532e84 100644 --- 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 @@ -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. * *

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 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 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); } } diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java new file mode 100644 index 0000000000..0be5357c69 --- /dev/null +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java @@ -0,0 +1,127 @@ +/* + * 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.sim; + +import static frc.robot.Constants.*; + +import edu.wpi.first.math.geometry.Pose2d; +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.util.Units; +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; + +/** + * This class handles the simulation of the camera and vision target on the field. Updating this + * class will update the camera data, reflecting what the simulated camera sees. + * + *

This class and its methods are only relevant during simulation. While on the real robot, the + * real camera data is used instead. + */ +public class VisionSim { + // ----- Simulation specific constants + // 2020 High goal target shape + // See + // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf + // page 208 + private static final TargetModel kTargetModel = + 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)))); + + // Simulated camera properties. These can be set to mimic your actual camera. + private static final int kResolutionWidth = 640; // pixels + private static final int kResolutionHeight = 480; // pixels + private static final Rotation2d kFOVDiag = Rotation2d.fromDegrees(100.0); // degrees + private static final double kAvgErrorPx = 0.2; + private static final double kErrorStdDevPx = 0.05; + private static final double kFPS = 25; + private static final double kAvgLatencyMs = 30; + private static final double kLatencyStdDevMs = 4; + private static final double kMinTargetArea = 0.1; // percentage (0 - 100) + private static final double kMaxLEDRange = 15; // meters + // ----- + + // A simulated vision system which handles simulated cameras and targets. + private VisionSystemSim visionSim; + // The simulation of our PhotonCamera, which will simulate camera frames and target info. + private PhotonCameraSim cameraSim; + + public VisionSim(String name, PhotonCamera camera) { + visionSim = new VisionSystemSim(name); + // Make the vision target visible to this simulated field. + var visionTarget = new VisionTargetSim(TARGET_POSE, kTargetModel); + visionSim.addVisionTargets(visionTarget); + + // Create simulated camera properties from our defined constants. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(kResolutionWidth, kResolutionHeight, kFOVDiag); + cameraProp.setCalibError(kAvgErrorPx, kErrorStdDevPx); + cameraProp.setFPS(kFPS); + cameraProp.setAvgLatencyMs(kAvgLatencyMs); + cameraProp.setLatencyStdDevMs(kLatencyStdDevMs); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + cameraSim = new PhotonCameraSim(camera, cameraProp, kMinTargetArea, kMaxLEDRange); + + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera( + cameraSim, + new Transform3d( + new Translation3d(0, 0, CAMERA_HEIGHT_METERS), + new Rotation3d(0, -CAMERA_PITCH_RADIANS, 0))); + + cameraSim.enableDrawWireframe(true); + } + + /** + * Update the simulated camera data based on its new field pose. + * + * @param simRobotPose The pose of the simulated robot + */ + public void update(Pose2d simRobotPose) { + visionSim.update(simRobotPose); + } + + /** + * 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) { + visionSim.resetRobotPose(pose); + } +}