mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
[examples] Re-organize simaimandrange (#937)
This commit is contained in:
amquake
@@ -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;
|
||||
// ----------
|
||||
}
|
||||
@@ -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.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 @@ import org.photonvision.PhotonUtils;
|
||||
* 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 class Robot extends TimedRobot {
|
||||
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 class Robot extends TimedRobot {
|
||||
}
|
||||
} 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 class Robot extends TimedRobot {
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
// PWM handles for getting commanded motor controller speeds
|
||||
private final PWMSim leftLeader;
|
||||
private final PWMSim rightLeader;
|
||||
|
||||
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);
|
||||
|
||||
// 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 class DrivetrainSim {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user