From 958bc92ca0a1c0b4cf16c25d83a751ce95f83b0d Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Wed, 18 Feb 2026 17:57:06 -0500 Subject: [PATCH] Some additional PhotonVision/Drivetrain work, and a terrible auto path --- .../My Bumpers Arent All The Way Over.path | 151 ++++++++++++++++++ .../frc/robot/constants/HoodConstants.java | 30 ++++ .../java/frc/robot/subsystems/Drivetrain.java | 63 +++----- .../frc/robot/subsystems/PhotonVision.java | 147 ++++++----------- 4 files changed, 246 insertions(+), 145 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path diff --git a/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path b/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path new file mode 100644 index 0000000..74dcf9e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path @@ -0,0 +1,151 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.572076308784383, + "y": 3.3951907719609573 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1112866015971603, + "y": 2.6628305235137533 + }, + "prevControl": { + "x": 2.8612866015971603, + "y": 2.6628305235137533 + }, + "nextControl": { + "x": 3.3612866015971603, + "y": 2.6628305235137533 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.839529724933453, + "y": 2.357009760425909 + }, + "prevControl": { + "x": 5.055196983507387, + "y": 2.7986728575396165 + }, + "nextControl": { + "x": 6.668464951197871, + "y": 1.890230700976042 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.612129547471162, + "y": 0.8279059449866895 + }, + "prevControl": { + "x": 6.3623142538133335, + "y": 0.8182976644613882 + }, + "nextControl": { + "x": 7.030621118012423, + "y": 0.844001774622892 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.189520851818989, + "y": 0.8279059449866895 + }, + "prevControl": { + "x": 8.08578683847011, + "y": 0.4907704016028375 + }, + "nextControl": { + "x": 8.318287488908608, + "y": 1.246397515527949 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.189520851818989, + "y": 7.111241666666667 + }, + "prevControl": { + "x": 8.36001268105017, + "y": 7.294087328812116 + }, + "nextControl": { + "x": 6.244366666666667, + "y": 5.025141666666666 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.518108333333333, + "y": 5.592016666666666 + }, + "prevControl": { + "x": 3.205916666666667, + "y": 5.644925000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 5, + "rotationDegrees": -135.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -133.87669728592465 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index f0380ec..772a2fa 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -1,10 +1,17 @@ package frc.robot.constants; +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; +import java.io.IOException; +import java.nio.file.Path; + import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.Filesystem; public class HoodConstants { // TODO Real Values @@ -46,5 +53,28 @@ public class HoodConstants { .positionWrappingInputRange(0, Math.PI * 2) .feedForward .sva(kS, kV, kA); + + + File interpolatorFile = Path.of( + Filesystem.getDeployDirectory().getAbsolutePath().toString(), + "interpolatorData.csv" + ).toFile(); + + if(interpolatorFile.exists()) { + try (BufferedReader reader = new BufferedReader(new FileReader(interpolatorFile))) { + reader.lines().forEach((s) -> { + if(s.trim() != "") { //Empty or whitespace line protection + String[] lineSplit = s.split(","); + + kDistanceToAngle.put( + Double.valueOf(lineSplit[0].replace("\"", "")), + Double.valueOf(lineSplit[1].replace("\"", "")) + ); + } + }); + } catch (IOException e) { + // This condition is never reached because of the if exists line above + } + } } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 3766c8c..8727052 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -198,27 +198,7 @@ public class Drivetrain extends SubsystemBase { * @return A complete Command structure that performs the specified action */ public Command rotateToPose(Pose2d targetPose, boolean rotate180) { - return runOnce(yawRotationController::reset).andThen( - drive( - () -> 0, - () -> 0, - () -> { - Rotation2d targetRotation = new Rotation2d( - targetPose.getX() - getPose().getX(), - targetPose.getY() - getPose().getY() - ); - - if(rotate180) { - targetRotation = targetRotation.rotateBy(Rotation2d.k180deg); - } - - return yawRotationController.calculate( - getHeading().getRadians(), - targetRotation.getRadians() - ); - }, - () -> true - )) + return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180) .until(yawRotationController::atSetpoint); } @@ -291,32 +271,27 @@ public class Drivetrain extends SubsystemBase { ); } - // TODO check both cameras - /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) { - if (camera1 == null) { - return new PrintCommand("Camera 1 not available"); - } - - // TODO The process variable is different here than what these constants are used for, may need to use something different - PIDController controller = new PIDController( - AutoConstants.kPThetaController, - 0, - 0 - ); - - return runOnce(controller::reset).andThen( + /** + * A method to lock to a particular source of an external "yaw". The intent is for this yaw to be sourced from + * {@link frc.robot.subsystems.PhotonVision#getBestYawForTag(int)} which generates a "yaw" for a particular tag as referenced + * from the center point of the cameras image frame. The objective being to "0 the source" using a PID Controller, or in + * other terms, to center the provided tag in the camera's image frame. + * + * @param yaw The "yaw" of the tag source relative to the center of the image frame + * @param xSpeed The X (forward/backward) translational speed of the robot + * @param ySpeed The Y (left/right) translational speed of the robot + * @return A complete Command structure that performs the specified action + */ + public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) { + return runOnce(yawRotationController::reset).andThen( drive( - xSpeed, + xSpeed, ySpeed, - () -> { - OptionalDouble tagYaw = camera1.getTagYawByID(tagID); - - return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0)); - }, - () -> false - ) + () -> yawRotationController.calculate(yaw.getAsDouble(), 0), + () -> true + ) ); - }*/ + } public Command drivePathPlannerPath(PathPlannerPath path) { if(AutoConstants.kAutoConfigOk) { diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 2358196..97fe420 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -3,6 +3,7 @@ package frc.robot.subsystems; import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.OptionalDouble; import java.util.function.Consumer; import org.photonvision.EstimatedRobotPose; @@ -138,8 +139,51 @@ public class PhotonVision extends SubsystemBase { return Optional.of(Pose3d.kZero .plus(bestCameraToTarget.inverse()) .relativeTo(CompetitionConstants.kTagLayout.getOrigin()) - .plus(config.robotToCamera().inverse()) - .plus(CompetitionConstants.kRobotToShooter)); + .plus(config.robotToCamera().inverse())); + } + + /** + * Returns the best yaw for a given AprilTag ID as seen by the cameras on the robot. + * + * All cameras fields of view are observed, if no camera can see the given tag ID, this + * method will return OptionalDouble.empty() + * + * Note that this method has no minimum confidence threshold for a tag. This means that + * if one camera thinks it sees the tag, even with very low confidence, it'll still + * return some sort of yaw value. + * + * Note that the yaw value here is the yaw of the observed tag relative to the center + * of the cameras image frame. + * + * @param tagID The ID of the tag to look for in the latest results from all cameras + * @return An OptionalDouble object containing a Double representing the described yaw + * of the AprilTag specified, or OptionalDouble.empty() if the tag is not present + * anywhere in the robots field of view + */ + public OptionalDouble getBestYawForTag(int tagID) { + double bestTagYaw = -1; + float bestConfidence = -1; + + for(PhotonPipelineResult result: latestResults) { + if(result == null) { + continue; + } + + for(PhotonTrackedTarget target: result.getTargets()) { + if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectClassID()) { + continue; + } + + bestTagYaw = target.getYaw(); + bestConfidence = target.getDetectedObjectConfidence(); + } + } + + if(bestConfidence == -1) { + return OptionalDouble.empty(); + } + + return OptionalDouble.of(bestTagYaw); } /** @@ -158,103 +202,4 @@ public class PhotonVision extends SubsystemBase { public void addPoseEstimateConsumer(Consumer consumer) { poseEstimateConsumers.add(consumer); } - - /*public Trigger tagPrescenseTrigger(int targetTag) { - return new Trigger(() -> { - return List.of(latestResults).stream() - .filter((p) -> p != null) - .anyMatch((p) -> { - return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> { - return i == targetTag; - }); - }); - }); - }*/ -/* - @Override - public OptionalDouble getTagDistanceFromCameraByID(int id) { - if (latestResult == null) { - return OptionalDouble.empty(); - } - - if (!latestResult.hasTargets()) { - return OptionalDouble.empty(); - } - - Optional desiredTarget = getTargetFromList(latestResult.getTargets(), id); - - if (desiredTarget.isEmpty()) { - return OptionalDouble.empty(); - } - - return OptionalDouble.of( - PhotonUtils.calculateDistanceToTargetMeters( - cameraHeightMeters, - CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(), - cameraPitchRadians, - Units.degreesToRadians(desiredTarget.get().getPitch())) - ); - } - - @Override - public OptionalDouble getTagPitchByID(int id) { - if(latestResult == null) { - OptionalDouble.empty(); - } - - if (!latestResult.hasTargets()) { - return OptionalDouble.empty(); - } - - Optional desiredTarget = getTargetFromList(latestResult.getTargets(), id); - - if (desiredTarget.isEmpty()) { - return OptionalDouble.empty(); - } - - return OptionalDouble.of( - desiredTarget.get().getPitch() - ); - } - - @Override - public OptionalDouble getTagYawByID(int id) { - if(latestResult == null) { - OptionalDouble.empty(); - } - - if (!latestResult.hasTargets()) { - return OptionalDouble.empty(); - } - - Optional desiredTarget = getTargetFromList(latestResult.getTargets(), id); - - if (desiredTarget.isEmpty()) { - return OptionalDouble.empty(); - } - - return OptionalDouble.of( - desiredTarget.get().getYaw() - ); - } - - private Optional getTargetFromList(List targets, int id) { - for (PhotonTrackedTarget target : targets) { - if (target.getFiducialId() == id) { - return Optional.of(target); - } - } - - return Optional.empty(); - } - - @Override - public int[] getVisibleTagIDs() { - if(latestResult == null) { - return new int[] {}; - } - - return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray(); - } - */ }