diff --git a/src/main/java/com/team6962/lib/vision/SphereCameraConstants.java b/src/main/java/com/team6962/lib/vision/SphereCameraConstants.java index ee68a14b..b69c2d22 100644 --- a/src/main/java/com/team6962/lib/vision/SphereCameraConstants.java +++ b/src/main/java/com/team6962/lib/vision/SphereCameraConstants.java @@ -43,6 +43,9 @@ public class SphereCameraConstants implements Cloneable { */ public Distance MaxDetectionRange = Meters.of(18.37); + /** Maximum range at which sphere pieces can be detected in simulation. */ + public Distance MaxSimulatedDetectionRange = Meters.of(8); + /** * Tolerance for sphere detection algorithm. This affects how closely a detected object must match * a spherical shape to be considered a sphere piece. @@ -170,6 +173,17 @@ public SphereCameraConstants withMaxDetectionRange(Distance maxRange) { return this; } + /** + * Sets the maximum range at which sphere pieces can be detected in simulation. + * + * @param maxSimulatedRange The maximum simulated detection range. + * @return This SphereCameraConstants instance for method chaining. + */ + public SphereCameraConstants withMaxSimulatedDetectionRange(Distance maxSimulatedRange) { + this.MaxSimulatedDetectionRange = maxSimulatedRange; + return this; + } + /** * Sets the tolerance for sphere detection algorithm. * diff --git a/src/main/java/com/team6962/lib/vision/SphereClumpLocalization.java b/src/main/java/com/team6962/lib/vision/SphereClumpLocalization.java index 413214a5..5fa1f7fc 100644 --- a/src/main/java/com/team6962/lib/vision/SphereClumpLocalization.java +++ b/src/main/java/com/team6962/lib/vision/SphereClumpLocalization.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.ArrayList; import java.util.List; @@ -42,6 +43,12 @@ public class SphereClumpLocalization extends SubsystemBase { /** Cached clump position */ private Translation2d cachedClumpPosition; + /** + * The list of simulated sphere positions for testing purposes. This allows us to bypass the + * camera processing and directly set the detected sphere positions in simulation. + */ + private List simulatedSpherePositions = new ArrayList<>(); + /** * Construct the sphere clump localization subsystem. * @@ -51,7 +58,10 @@ public class SphereClumpLocalization extends SubsystemBase { public SphereClumpLocalization( MotionSwerveDrive swerveDrive, SphereCameraConstants cameraConstants) { this.swerveDrive = swerveDrive; - this.camera = !cameraConstants.Name.equals("") ? new PhotonCamera(cameraConstants.Name) : null; + this.camera = + !cameraConstants.Name.equals("") && RobotBase.isReal() + ? new PhotonCamera(cameraConstants.Name) + : null; this.cameraConstants = cameraConstants; } @@ -61,7 +71,7 @@ public SphereClumpLocalization( */ @Override public void periodic() { - if (camera == null) return; + if (cameraConstants.Name.equals("")) return; cachedClumpPosition = computeClumpPosition(); @@ -85,6 +95,12 @@ public Translation2d getClumpPosition() { return cachedClumpPosition; } + public void setSimulatedSpherePositions(List spherePositions) { + if (RobotBase.isSimulation()) { + simulatedSpherePositions = spherePositions; + } + } + /** * Query the camera for targets and compute a best-guess clump position. * @@ -102,65 +118,154 @@ public Translation2d getClumpPosition() { * @return best estimated clump position as a Translation2d, or null if none found/valid */ private Translation2d computeClumpPosition() { - // TODO: Switch to getAllUnreadResults() - PhotonPipelineResult result = camera.getLatestResult(); - DogLog.log("Vision/Cameras/" + camera.getName() + "/Spheres/HasTargets", result.hasTargets()); + PhotonPipelineResult result = null; + String cameraName = cameraConstants.Name; - if (!result.hasTargets()) { - DogLog.log("Vision/Cameras/" + camera.getName() + "/Spheres/Count", 0); + if (RobotBase.isReal()) { + // TODO: Switch to getAllUnreadResults() + result = camera.getLatestResult(); + DogLog.log("Vision/Cameras/" + cameraName + "/Spheres/HasTargets", result.hasTargets()); + } + + DogLog.log( + "Vision/Cameras/" + cameraName + "/Pose", + swerveDrive.getPosition3d().plus(cameraConstants.RobotToCameraTransform)); + + if (RobotBase.isReal() && !result.hasTargets()) { + DogLog.log("Vision/Cameras/" + cameraName + "/Spheres/Count", 0); return null; } // Get all sphere locations from tracked targets - List targets = result.getTargets(); + List sphereLocations3d; + + if (RobotBase.isReal()) { + sphereLocations3d = computeRealSpherePositions(result); + } else { + sphereLocations3d = computeSimulatedSpherePositions(); + } + + List sphereLocations2d = + sphereLocations3d.stream().map(t -> t.toTranslation2d()).toList(); - List sphereLocations2d = new ArrayList<>(); - List sphereLocations3d = new ArrayList<>(); + // Publish all detected spheres for visualization (Pose2d list) + logSpheres(sphereLocations2d); - int sphereIndex = 0; - int validSphereCount = 0; + // Select the detection with the smallest summed planar distance to other detections. + // This heuristically picks a detection near the clump center. + return findClump(sphereLocations2d); + } + + private List computeRealSpherePositions(PhotonPipelineResult result) { + List spherePositions = new ArrayList<>(); + + List targets = result.getTargets(); + + int index = 0; for (PhotonTrackedTarget target : targets) { if (target == null) continue; - // Increment sphere index but capture its initial value for logging - int thisSphereIndex = sphereIndex; - sphereIndex++; - - // Convert tracked target into a 3D field-relative position - Translation3d sphereLocation3d = getSpherePosition(target, thisSphereIndex); + Translation3d position = getSpherePosition(target, index); DogLog.log( - "Vision/Cameras/" + camera.getName() + "/Spheres/" + thisSphereIndex + "/IsValid", - sphereLocation3d != null); + "Vision/Cameras/" + cameraConstants.Name + "/Spheres/" + index + "/IsValid", + position != null); + + if (position != null) { + spherePositions.add(position); + + DogLog.log( + "Vision/Cameras/" + + cameraConstants.Name + + "/Spheres/" + + index + + "/FieldRelativePosition", + new Pose3d(position, new Rotation3d())); + } - if (sphereLocation3d == null) continue; + index++; + } - validSphereCount++; + return spherePositions; + } - sphereLocations3d.add(sphereLocation3d); - sphereLocations2d.add(sphereLocation3d.toTranslation2d()); + /** + * Computes simulated sphere positions, filtering out any that are outside the camera's maximum + * simulated detection range or outside the camera's field of view based on the robot's current + * position and orientation. This allows us to test the clump localization logic in simulation by + * directly setting sphere positions and having the subsystem process them as if they were + * detected by the camera. + * + * @return a list of valid simulated sphere positions + */ + private List computeSimulatedSpherePositions() { + List spherePositions = new ArrayList<>(); + + int index = 0; + + for (Translation3d position : simulatedSpherePositions) { + if (swerveDrive.getPosition2d().getTranslation().getDistance(position.toTranslation2d()) + > Math.min( + cameraConstants.MaxSimulatedDetectionRange.in(Meters), + cameraConstants.MaxDetectionRange.in(Meters))) { + continue; // Skip simulated spheres that are out of range + } + + Pose3d robotPose = swerveDrive.getPosition3d(); + Pose3d cameraPose = robotPose.plus(cameraConstants.RobotToCameraTransform); + Pose3d sphereRelativeToCamera = new Pose3d(position, new Rotation3d()).relativeTo(cameraPose); + + Angle yaw = + Radians.of(Math.atan2(sphereRelativeToCamera.getY(), sphereRelativeToCamera.getX())); + Angle pitch = + Radians.of(Math.atan2(sphereRelativeToCamera.getZ(), sphereRelativeToCamera.getX())); + + if (Math.abs(yaw.in(Degrees)) > cameraConstants.FOVWidth.getDegrees() / 2 + || Math.abs(pitch.in(Degrees)) > cameraConstants.FOVHeight.getDegrees() / 2) { + continue; // Skip simulated spheres that are outside the camera's FOV + } + + spherePositions.add(position); DogLog.log( - "Vision/Cameras/" - + camera.getName() - + "/Spheres/" - + thisSphereIndex - + "/FieldRelativePosition", - new Pose3d(sphereLocation3d, new Rotation3d())); + "Vision/Cameras/" + cameraConstants.Name + "/Spheres/" + index + "/FieldRelativePosition", + new Pose3d(position, new Rotation3d())); + + index++; } - // Publish all detected spheres for visualization (Pose2d list) + return spherePositions; + } + + /** + * Helper method to publish detected sphere positions to the field logger for visualization. + * Converts a list of Translation2d positions into Pose2d with zero rotation and updates a field + * object named "Sphere". Also logs the count of detected spheres. + * + * @param spheres a list of 2D positions of detected spheres + */ + private void logSpheres(List spheres) { swerveDrive .getFieldLogger() .getField() .getObject("Sphere") - .setPoses(sphereLocations2d.stream().map(t -> new Pose2d(t, new Rotation2d())).toList()); + .setPoses(spheres.stream().map(t -> new Pose2d(t, new Rotation2d())).toList()); - DogLog.log("Vision/Cameras/" + camera.getName() + "/Spheres/Count", validSphereCount); + DogLog.log("Vision/Cameras/" + cameraConstants.Name + "/Spheres/Count", spheres.size()); + } - // Select the detection with the smallest summed planar distance to other detections. - // This heuristically picks a detection near the clump center. + /** + * Given a list of detected sphere positions, find the one that is most likely to be at the center + * of a clump by selecting the position with the lowest total distance to the other positions. + * This is a heuristic that assumes the clump center will be the detection closest to the other + * detections. + * + * @param sphereLocations2d a list of 2D positions of detected spheres + * @return the 2D position of the sphere most likely at the center of the clump, or null if + * spherePositions was empty + */ + private Translation2d findClump(List sphereLocations2d) { Translation2d bestPosition = null; double lowestTotalDistance = Double.MAX_VALUE; @@ -238,7 +343,7 @@ private Translation3d getSpherePosition(PhotonTrackedTarget target, int loggingI if (loggingIndex != -1) { DogLog.log( "Vision/Cameras/" - + camera.getName() + + cameraConstants.Name + "/Spheres/" + loggingIndex + "/CameraToTargetDistance", @@ -261,7 +366,7 @@ private Translation3d getSpherePosition(PhotonTrackedTarget target, int loggingI if (loggingIndex != -1) { DogLog.log( "Vision/Cameras/" - + camera.getName() + + cameraConstants.Name + "/Spheres/" + loggingIndex + "/CameraRelativePosition", @@ -277,7 +382,7 @@ private Translation3d getSpherePosition(PhotonTrackedTarget target, int loggingI if (loggingIndex != -1) { DogLog.log( "Vision/Cameras/" - + camera.getName() + + cameraConstants.Name + "/Spheres/" + loggingIndex + "/RobotRelativePosition", diff --git a/src/main/java/frc/robot/auto/DriveToClump.java b/src/main/java/frc/robot/auto/DriveToClump.java index 4d307bfa..8dddd360 100644 --- a/src/main/java/frc/robot/auto/DriveToClump.java +++ b/src/main/java/frc/robot/auto/DriveToClump.java @@ -1,6 +1,11 @@ package frc.robot.auto; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.RobotContainer; @@ -51,10 +56,39 @@ public Command driveToClump() { return Commands.none(); } + Translation2d error = + fuelPosition.minus(robot.getSwerveDrive().getPosition2d().getTranslation()); + + double maxVelocity = + robot + .getConstants() + .getDrivetrainConstants() + .Driving + .MaxLinearVelocity + .in(MetersPerSecond); + double maxAcceleration = + robot + .getConstants() + .getDrivetrainConstants() + .Driving + .MaxLinearAcceleration + .in(MetersPerSecondPerSecond); + + double distance = error.getNorm(); + double finalSpeed = Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * distance)); + + ChassisSpeeds finalVelocity = + new ChassisSpeeds( + error.getX() / distance * finalSpeed, + error.getY() / distance * finalSpeed, + 0); + return Commands.either( - Commands.parallel( - robot.getIntakeRollers().run(robot.getIntakeRollers()::intake), - robot.getSwerveDrive().driveTo(fuelPosition)), + Commands.deadline( + robot + .getSwerveDrive() + .driveTo(new Pose2d(fuelPosition, error.getAngle()), finalVelocity), + robot.getIntakeRollers().intake()), Commands.none(), robot.getIntakeExtension()::isExtended); }, diff --git a/src/main/java/frc/robot/auto/ShooterFunctions.java b/src/main/java/frc/robot/auto/ShooterFunctions.java index f05aa887..8ad63d8e 100644 --- a/src/main/java/frc/robot/auto/ShooterFunctions.java +++ b/src/main/java/frc/robot/auto/ShooterFunctions.java @@ -29,8 +29,8 @@ public class ShooterFunctions { private static final String velocityPathHub = "flywheelvelocitydatahub.csv"; /** - * Path to the CSV file containing hood angle and flywheel velocity calibration data for passing. Formatted as Distance, - * Angle + * Path to the CSV file containing hood angle and flywheel velocity calibration data for passing. + * Formatted as Distance, Angle */ private static final String passingPath = "passingdata.csv"; diff --git a/src/main/java/frc/robot/constants/CompetitionBotConstants.java b/src/main/java/frc/robot/constants/CompetitionBotConstants.java index 2ed8d388..db6cb8ce 100644 --- a/src/main/java/frc/robot/constants/CompetitionBotConstants.java +++ b/src/main/java/frc/robot/constants/CompetitionBotConstants.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Hertz; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.Pounds; @@ -30,6 +31,10 @@ import com.team6962.lib.swerve.config.XBoxTeleopSwerveConstants; import com.team6962.lib.vision.AprilTagVisionConstants; import com.team6962.lib.vision.SphereCameraConstants; +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; import edu.wpi.first.math.system.plant.DCMotor; public class CompetitionBotConstants extends BaseRobotConstants { @@ -233,7 +238,22 @@ public AprilTagVisionConstants getAprilTagVisionConstants() { @Override public SphereCameraConstants getSphereCameraConstants() { - return super.getSphereCameraConstants(); + return super.getSphereCameraConstants() + .withName("Color-2") + .withClassId(0) + .withFOVHeight(Rotation2d.fromDegrees(48.9)) + .withFOVWidth(Rotation2d.fromDegrees(70)) + .withCameraHeightPixels(800) + .withCameraWidthPixels(1280) + .withFocalLengthX(907.41) + .withFocalLengthY(907.64) + .withMaxDetectionRange(Meters.of(18.37)) // diagonal length of the field + .withSphereDiameter(Inches.of(5.91)) + .withMaxTargets(50) // Temporary value until we tune object detection on the practice field + .withRobotToCameraTransform( + new Transform3d( + new Translation3d(Inches.of(16.25).in(Meters), 0, Inches.of(20.0).in(Meters)), + new Rotation3d(0, Math.PI / 6, 0))); } @Override diff --git a/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java b/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java index 606c8a26..17a7844d 100644 --- a/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java +++ b/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java @@ -111,6 +111,19 @@ public void periodic() { fuelCount = RobotVisualizationConstants.maxRetractedFuel; } + List fuelPositions = + robot + .getSwerveDrive() + .getSimulation() + .getMapleSim() + .getArena() + .getGamePiecesByType("Fuel") + .stream() + .map(gamePiece -> gamePiece.getPose3d().getTranslation()) + .toList(); + + robot.getFuelLocalization().setSimulatedSpherePositions(fuelPositions); + List additionalFuelPoses = new ArrayList<>(); for (int i = 0; i < fuelCount; i++) {