Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update Java Simulation Examples (#913)

Browse Source 
Removes apriltagExample and simposeest, replacing them with swervedriveposeestsim

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
amquake
2023-10-05 05:57:38 -07:00
committed by GitHub
parent 65d5494ab3
commit ce0d28da93
65 changed files with 1859 additions and 2011 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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

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

@@ -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);
}
}