diff --git a/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json b/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json index fa15d0e412..0a74652686 100644 --- a/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json +++ b/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2025", - "teamNumber": 1736 + "teamNumber": 4512 } diff --git a/photonlib-java-examples/aimandrange/gradlew b/photonlib-java-examples/aimandrange/gradlew old mode 100755 new mode 100644 diff --git a/photonlib-java-examples/aimandrange/simgui.json b/photonlib-java-examples/aimandrange/simgui.json index 1ca6446d51..fc015367da 100644 --- a/photonlib-java-examples/aimandrange/simgui.json +++ b/photonlib-java-examples/aimandrange/simgui.json @@ -1,11 +1,4 @@ { - "HALProvider": { - "Other Devices": { - "window": { - "visible": false - } - } - }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -114,38 +107,20 @@ }, "open": true }, - "Shuffleboard": { - "open": true - }, "SmartDashboard": { - "Drive": { - "open": true - }, "VisionSystemSim-main": { "open": true }, "open": true - }, - "out": { - "open": true } } }, "NetworkTables Info": { - "Clients": { - "open": true - }, - "Connections": { - "open": true - }, - "Server": { - "Publishers": { - "open": true - }, - "open": true - }, "visible": true }, + "NetworkTables View": { + "visible": false + }, "Plot": { "Plot <0>": { "plots": [ @@ -161,7 +136,7 @@ 0.0, 0.8500000238418579 ], - "height": 0, + "height": 338, "series": [ { "color": [ diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java index ced7969d25..da4d3d7fa0 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java @@ -42,7 +42,7 @@ public static class Vision { public static final String kCameraName = "YOUR CAMERA NAME"; // Cam mounted facing forward, half a meter forward of center, half a meter up from center, // pitched upward. - private static final double camPitch = Units.degreesToRadians(10.0); + private static final double camPitch = Units.degreesToRadians(30.0); public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java index 7da422649f..2d58a27479 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java @@ -26,139 +26,52 @@ import static frc.robot.Constants.Vision.*; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.cscore.OpenCvLoader; 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.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.drivetrain.SwerveDrive; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.PhotonUtils; public class Robot extends TimedRobot { - PhotonPoseEstimator casadiEstimator = - new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), - PoseStrategy.CASADI_MEME, - kRobotToCam); - PhotonPoseEstimator oldEstimator = - new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - kRobotToCam); - - StructPublisher poseOldPub; - StructPublisher poseNewPub; - - DoublePublisher oldDtPub; - DoublePublisher newDtPub; - private SwerveDrive drivetrain; private VisionSim visionSim; private PhotonCamera camera; - private XboxController controller; + private final double VISION_TURN_kP = 0.01; + private final double VISION_DES_ANGLE_deg = 0.0; + private final double VISION_STRAFE_kP = 0.5; + private final double VISION_DES_RANGE_m = 1.25; - private boolean FORCE_LOCAL = false; + private XboxController controller; @Override public void robotInit() { - if (isSimulation()) { - drivetrain = new SwerveDrive(); - } + drivetrain = new SwerveDrive(); camera = new PhotonCamera(kCameraName); - visionSim = new VisionSim(camera); - controller = new XboxController(0); - - if (isReal() || FORCE_LOCAL) { - poseOldPub = - NetworkTableInstance.getDefault() - .getStructTopic("/out/pose_legacy", Pose3d.struct) - .publish(); - poseNewPub = - NetworkTableInstance.getDefault() - .getStructTopic("/out/pose_constrained", Pose3d.struct) - .publish(); - newDtPub = NetworkTableInstance.getDefault().getDoubleTopic("/out/dt_constrained").publish(); - - OpenCvLoader.forceStaticLoad(); - } + visionSim = new VisionSim(camera); - if (isSimulation() && !FORCE_LOCAL) { - NetworkTableInstance.getDefault().stopServer(); - NetworkTableInstance.getDefault().setServer("10.17.36.2"); - NetworkTableInstance.getDefault().startClient4("matt-laptop"); - } + controller = new XboxController(0); } @Override public void robotPeriodic() { - if (isSimulation()) { - // Update drivetrain subsystem - drivetrain.periodic(); - - // Log values to the dashboard - drivetrain.log(); - } - - if (isReal() || FORCE_LOCAL) { - // System.out.println("I shouldn't be here"); - for (var result : camera.getAllUnreadResults()) { - Optional oldPoseEst = - oldEstimator.update(result, camera.getCameraMatrix(), camera.getDistCoeffs(), null); - if (oldPoseEst.isPresent()) { - Pose3d pose = oldPoseEst.get().estimatedPose; - poseOldPub.set(pose); - } + // Update drivetrain subsystem + drivetrain.periodic(); - var headingArr = - SmartDashboard.getNumberArray("VisionSystemSim-main/Sim Field/Robot", new double[0]); - if (headingArr.length != 3) break; - var heading = Rotation2d.fromDegrees(headingArr[2]); - System.out.println(heading.getDegrees()); - - // some drift lol - var offset = Rotation2d.fromDegrees((Math.random() - 0.5)); - var t1 = Timer.getFPGATimestamp(); - Optional newPoseEst = - casadiEstimator.update( - result, - camera.getCameraMatrix(), - camera.getDistCoeffs(), - new PhotonPoseEstimator.ConstrainedSolvepnpParams(true, heading.plus(offset), 100)); - var t2 = Timer.getFPGATimestamp(); - newDtPub.set(t2 - t1); - if (newPoseEst.isPresent() && newPoseEst.get().strategy == PoseStrategy.CASADI_MEME) { - Pose3d pose = newPoseEst.get().estimatedPose; - poseNewPub.set(pose); - SmartDashboard.putNumber("yaw_offset", offset.getDegrees()); - SmartDashboard.putNumber( - "yaw_error_deg", - heading - .minus(newPoseEst.get().estimatedPose.getRotation().toRotation2d()) - .getDegrees()); - } - } - } + // Log values to the dashboard + drivetrain.log(); } @Override public void disabledPeriodic() { - if (drivetrain != null) drivetrain.stop(); + drivetrain.stop(); } @Override @@ -173,8 +86,52 @@ public void teleopPeriodic() { double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed; double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed; + // Read in relevant data from the Camera + boolean targetVisible = false; + double targetYaw = 0.0; + double targetRange = 0.0; + var results = camera.getAllUnreadResults(); + if (!results.isEmpty()) { + // Camera processed a new frame since last + // Get the last one in the list. + var result = results.get(results.size() - 1); + if (result.hasTargets()) { + // At least one AprilTag was seen by the camera + for (var target : result.getTargets()) { + if (target.getFiducialId() == 7) { + // Found Tag 7, record its information + targetYaw = target.getYaw(); + targetRange = + PhotonUtils.calculateDistanceToTargetMeters( + 0.5, // Measured with a tape measure, or in CAD. + 1.435, // From 2024 game manual for ID 7 + Units.degreesToRadians(-30.0), // Measured with a protractor, or in CAD. + Units.degreesToRadians(target.getPitch())); + + targetVisible = true; + } + } + } + } + + // Auto-align when requested + if (controller.getAButton() && targetVisible) { + // Driver wants auto-alignment to tag 7 + // And, tag 7 is in sight, so we can turn toward it. + // Override the driver's turn and fwd/rev command with an automatic one + // That turns toward the tag, and gets the range right. + turn = + (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; + forward = + (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed; + } + // Command drivetrain motors based on target speeds drivetrain.drive(forward, strafe, turn); + + // Put debug information to the dashboard + SmartDashboard.putBoolean("Vision Target Visible", targetVisible); + SmartDashboard.putNumber("Vision Target Range (m)", targetRange); } @Override diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java index 89752ab0a5..ba9d9d01cc 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java @@ -48,7 +48,7 @@ public VisionSim(PhotonCamera cam_in) { visionSim.addAprilTags(kTagLayout); // Create simulated camera properties. These can be set to mimic your actual camera. var cameraProp = new SimCameraProperties(); - cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(90)); + cameraProp.setCalibration(320, 240, Rotation2d.fromDegrees(90)); cameraProp.setCalibError(0.35, 0.10); cameraProp.setFPS(70); cameraProp.setAvgLatencyMs(30);