Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Some additional PhotonVision/Drivetrain work, and a terrible auto path

Browse Source
This commit is contained in:
Bradley Bickford
2026-02-18 17:57:06 -05:00
parent 88c021f05e
commit 958bc92ca0
4 changed files with 246 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

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

147
src/main/java/frc/robot/subsystems/PhotonVision.java
View File

@@ -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<VisualPose> 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<PhotonTrackedTarget> 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<PhotonTrackedTarget> 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<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
if (desiredTarget.isEmpty()) {
return OptionalDouble.empty();
}
return OptionalDouble.of(
desiredTarget.get().getYaw()
);
}
private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> 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();
}
*/
}