From e6b8c515a249b98eea4560e5d7d7712fb070d857 Mon Sep 17 00:00:00 2001 From: Anay Nagar Date: Sat, 24 Feb 2024 12:08:30 -0800 Subject: [PATCH] Added vision consumer to replace Optional framework --- src/main/java/frc/robot/RobotContainer.java | 4 +++ .../frc/robot/subsystems/DriveSubsystem.java | 16 ++-------- .../frc/robot/subsystems/VisionSubsystem.java | 30 ++++++++++++++----- 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4044dd2..aca4e32 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.VisionSubsystem; /* * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -39,6 +40,7 @@ public class RobotContainer { private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); @@ -60,6 +62,8 @@ public RobotContainer() { new ReplanningConfig(true, true)), () -> false, m_robotDrive); + m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); + // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 6df1fae..b1e034b 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -4,8 +4,6 @@ package frc.robot.subsystems; -import java.util.Optional; - import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.MathUtil; @@ -71,8 +69,6 @@ public class DriveSubsystem extends SubsystemBase { m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs, VisionConstants.kVisionSTDDevs); - private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); - private final Field2d m_field = new Field2d(); /** Creates a new DriveSubsystem. */ @@ -96,14 +92,6 @@ public void periodic() { m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), m_swerveModulePositions); - Optional latestReading = m_visionSubsystem.getMeasurement(); - - // SmartDashboard.putBoolean("reading present", latestReading.isPresent()); - - if (latestReading.isPresent()) { - m_poseEstimator.addVisionMeasurement(latestReading.get().pose.toPose2d(), latestReading.get().timestamp); - } - m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); SmartDashboard.putNumber("gyro angle", m_gyro.getAngle()); @@ -213,8 +201,8 @@ public void zeroHeading() { m_gyroAngle = 0; } - public void addVisionMeasurement(Pose2d pose, double timestamp) { - m_poseEstimator.addVisionMeasurement(pose, timestamp); + public void addVisionMeasurement(Measurement measurement) { + m_poseEstimator.addVisionMeasurement(measurement.pose.toPose2d(), measurement.timestamp); } /** diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index e22d3e0..ebf72ce 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -5,7 +5,8 @@ package frc.robot.subsystems; import java.util.Arrays; -import java.util.Optional; +import java.util.List; +import java.util.function.Consumer; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; @@ -49,12 +50,14 @@ */ public class VisionSubsystem extends SubsystemBase { - NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight"); + private NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight"); private final DoubleArraySubscriber m_botPose; private final IntegerSubscriber m_tv; + private List> m_consumerList; + /** Creates a new Limelight. */ public VisionSubsystem() { // Provide the limelight with the camera pose relative to the center of the @@ -66,20 +69,33 @@ public VisionSubsystem() { m_tv = m_visionNetworkTable.getIntegerTopic("tv").subscribe(0); } + /** Add a consumer, which the vision subsystem will push an update to every time there is an updated measurement */ + public void addConsumer(Consumer consumer) { + m_consumerList.add(consumer); + } + @Override public void periodic() { // This method will be called once per scheduler run + Measurement latestMeasurement = getMeasurement(); + + if (latestMeasurement != null) { + for (Consumer consumer : m_consumerList) { + consumer.accept(latestMeasurement); + } + } + SmartDashboard.putBoolean("Limelight Has Target", m_tv.get() == 1); } - public Optional getMeasurement() { + public Measurement getMeasurement() { TimestampedDoubleArray[] updates = m_botPose.readQueue(); // If we have had no updates since the last time this method ran then return // nothing if (updates.length == 0) { - return Optional.empty(); + return null; } TimestampedDoubleArray update = updates[updates.length - 1]; @@ -87,7 +103,7 @@ public Optional getMeasurement() { // If the latest update is empty or we don't see an april tag then return // nothing if (Arrays.equals(update.value, new double[6]) || m_tv.get() == 0) { - return Optional.empty(); + return null; } double x = update.value[0]; @@ -100,10 +116,10 @@ public Optional getMeasurement() { double timestamp = Timer.getFPGATimestamp() - (update.value[6] / 1000.0); Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw)); - return Optional.of(new Measurement( + return new Measurement( timestamp, pose, - VisionConstants.kVisionSTDDevs)); + VisionConstants.kVisionSTDDevs); } public static class Measurement {