From eb1490f48d6629206f8dbf53ef0764577b475ff1 Mon Sep 17 00:00:00 2001 From: Timothy Bevis Date: Tue, 18 Feb 2025 21:20:19 -0500 Subject: [PATCH] Vision additions w/ testing (probs not right but getting there...) --- .../chopshoplib/maps/VisionMap.java | 15 ++++++++----- src/main/java/frc/robot/Robot.java | 5 ++--- src/main/java/frc/robot/Vision.java | 3 +++ src/main/java/frc/robot/maps/ColdStart.java | 9 ++++++-- src/main/java/frc/robot/subsystems/Drive.java | 22 ++++++++++++++----- 5 files changed, 38 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java b/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java index 803c221..1073413 100644 --- a/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java +++ b/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java @@ -6,10 +6,12 @@ import java.util.List; import java.util.Map; +import org.littletonrobotics.junction.Logger; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.estimator.PoseEstimator; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; public class VisionMap { @@ -45,8 +47,7 @@ public VisionMap(final List visionSources) { * @param Estimator wheel type. * @param estimator The WPIlib estimator object. */ - public void updateData(Data data) { - data.targets.clear(); + public void updateData(Data data) { for (var source : this.visionSources) { var results = source.camera.getAllUnreadResults(); if (!results.isEmpty()) { @@ -58,10 +59,14 @@ public void updateData(Data data) { est.timestampSeconds); }); // Now copy the targets that are found - // data.targets.add(List.copyOf(latestResult.targets)); + if (latestResult.targets.size() != 0) { + data.targets.clear(); + } for (var target : latestResult.targets) { int targetID = target.getFiducialId(); + Logger.recordOutput("target" + targetID, target.bestCameraToTarget); target.bestCameraToTarget = target.bestCameraToTarget.plus(source.robotToCam); + Logger.recordOutput("targetOffset" + targetID, target.bestCameraToTarget); if (data.targets.containsKey(target.getFiducialId())) { data.targets.get(targetID).add(target); } else { @@ -74,8 +79,8 @@ public void updateData(Data data) { } } - public static class Data { - public PoseEstimator estimator; + public static class Data { + public SwerveDrivePoseEstimator estimator; public Map> targets = new HashMap<>(); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b6eef40..aa6e9c0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -134,9 +134,8 @@ public void configureButtonBindings() { driveController.back().onTrue(commandSequences.resetAll()); driveController.leftBumper() .whileTrue(drive.robotCentricDrive()); - driveController.rightBumper().whileTrue(drive.aimAtReefCenter()); - driveController.rightTrigger().whileTrue(drive.alignToReefBranch(Branch.RIGHT_BRANCH)); - driveController.leftTrigger().whileTrue(drive.alignToReefBranch(Branch.LEFT_BRANCH)); + driveController.rightTrigger().whileTrue(drive.moveToBranch(Branch.RIGHT_BRANCH)); + driveController.leftTrigger().whileTrue(drive.moveToBranch(Branch.LEFT_BRANCH)); copilotController.a().onTrue(commandSequences.intake()); diff --git a/src/main/java/frc/robot/Vision.java b/src/main/java/frc/robot/Vision.java index dc4068f..40ae00e 100644 --- a/src/main/java/frc/robot/Vision.java +++ b/src/main/java/frc/robot/Vision.java @@ -131,6 +131,9 @@ public Transform2d pickBestReefLocation(Map> // Find the closest april tag target // For now we only look at a single camera's data // In the future we may want to combine the pose data for the april tags? + if (targets.size() == 0) { + return new Transform2d(); + } var closestTarget = Collections.min( targets.entrySet(), Comparator.comparing((Map.Entry> entry) -> { diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java index 663da38..c567a12 100644 --- a/src/main/java/frc/robot/maps/ColdStart.java +++ b/src/main/java/frc/robot/maps/ColdStart.java @@ -27,6 +27,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; @@ -117,8 +118,12 @@ public SwerveDriveMap getDriveMap() { public VisionMap getVisionMap() { return new VisionMap( - new CameraSource("FLCamera", new Transform3d()), - new CameraSource("FRCamera", new Transform3d())); + new CameraSource("FLCamera", + new Transform3d(Units.inchesToMeters(9.43), Units.inchesToMeters(10.72), + Units.inchesToMeters(8.24), new Rotation3d(0, -68, -16.76))), + new CameraSource("FRCamera", new Transform3d(Units.inchesToMeters(-10.72), Units.inchesToMeters(9.43), + Units.inchesToMeters(8.24), + new Rotation3d(0, Units.degreesToRadians(-68), Units.degreesToRadians(16.76))))); } @Override diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index b2a24dd..1e867f3 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -31,13 +31,14 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Vision; +import frc.robot.FieldConstants.Reef; import frc.robot.Vision.Branch; public class Drive extends LoggedSubsystem { public final SwerveDriveKinematics kinematics; private final VisionMap visionMap; - private final VisionMap.Data visionData = new VisionMap.Data<>(); + private final VisionMap.Data visionData = new VisionMap.Data(); private final Vision vision = new Vision(); private final double maxDriveSpeedMetersPerSecond; @@ -83,6 +84,8 @@ public Drive(SwerveDriveMap map, DoubleSupplier xSpeed, DoubleSupplier ySpeed, D VecBuilder.fill(0.02, 0.02, 0.01), VecBuilder.fill(0.1, 0.1, 0.01)); + visionData.estimator = estimator; + AutoBuilder.configure(estimator::getEstimatedPosition, this::setPose, this::getSpeeds, @@ -179,6 +182,8 @@ public void periodic() { Logger.recordOutput("Estimator Pose", estimator.getEstimatedPosition()); Logger.recordOutput("Pose Angle", estimator.getEstimatedPosition().getRotation()); Logger.recordOutput("Robot Rotation Gyro", getMap().gyro.getRotation2d()); + Logger.recordOutput("Target Branch", targetBranch); + } private double calculateRotationSpeed(double targetAngleDegrees) { @@ -209,13 +214,18 @@ private void periodicMove(final double xSpeed, final double ySpeed, final double rotationSpeed = calculateRotateSpeedToTarget(aimTarget::get); } if (targetBranch != Branch.NONE) { - alignToReefBranch(targetBranch); vision.filterReefTags(isBlueAlliance, visionData.targets); + Transform2d reefLocation = vision.pickBestReefLocation(visionData.targets); Transform2d robotToBranch = vision - .adjustTranslationForBranch(vision.pickBestReefLocation(visionData.targets), targetBranch); - translateXSpeed = translationPID_X.calculate(robotToBranch.getX()); - translateYSpeed = translationPID_Y.calculate(robotToBranch.getY()); - rotationSpeed = rotationPID.calculate(robotToBranch.getRotation().getDegrees()); + .adjustTranslationForBranch(reefLocation, targetBranch); + Logger.recordOutput("Reef Location Transform2d", estimator.getEstimatedPosition().plus(reefLocation)); + Logger.recordOutput("Robot To Branch Transform2d", estimator.getEstimatedPosition().plus(robotToBranch)); + Pose2d robotPose = estimator.getEstimatedPosition(); + translateXSpeed = translationPID_X.calculate(robotPose.getX(), robotPose.getX() + robotToBranch.getX()); + translateYSpeed = translationPID_Y.calculate(robotPose.getY(), robotPose.getY() + robotToBranch.getY()); + rotationSpeed = rotationPID.calculate(robotPose.getRotation().getDegrees(), + robotPose.getRotation().getDegrees() - robotToBranch.getRotation().getDegrees()); + Logger.recordOutput("Robot To Branch Angle", robotToBranch.getRotation().getDegrees()); } move(translateXSpeed, translateYSpeed, rotationSpeed, isRobotCentric);