From 6753bac5c3649ef8f9edf513e8065c177d25f996 Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Tue, 17 Sep 2024 18:58:24 -0700 Subject: [PATCH 1/2] work on limelight --- .../localization/LocalizationSubsystem.java | 11 ++-- src/main/java/frc/robot/vision/Limelight.java | 30 +++++++++++ .../frc/robot/vision/VisionSubsystem.java | 50 +++++++------------ 3 files changed, 55 insertions(+), 36 deletions(-) create mode 100644 src/main/java/frc/robot/vision/Limelight.java diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index 4448e29..f113cee 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -12,6 +12,8 @@ import frc.robot.vision.VisionResult; import frc.robot.vision.VisionSubsystem; import java.util.Optional; +import java.util.List; +import java.util.ArrayList; public class LocalizationSubsystem extends StateMachine { private final ImuSubsystem imu; @@ -20,7 +22,7 @@ public class LocalizationSubsystem extends StateMachine { private final TimeInterpolatableBuffer poseHistory = TimeInterpolatableBuffer.createBuffer(1.5); private double lastAddedVisionTimestamp = 0; - private Optional latestResult = Optional.empty(); + private List latestResult = new ArrayList<>(); public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision) { super(SubsystemPriority.LOCALIZATION, LocalizationState.DEFAULT_STATE); @@ -41,9 +43,8 @@ public Pose2d getPose() { @Override public void robotPeriodic() { super.robotPeriodic(); - - if (latestResult.isPresent()) { - var results = latestResult.get(); +for (int resultnum : latestResult.size()) + results = latestResult.get(resultnum); Pose2d visionPose = results.pose(); double visionTimestamp = results.timestamp(); @@ -59,7 +60,7 @@ public void robotPeriodic() { RobotConfig.get().vision().xyStdDev(), RobotConfig.get().vision().thetaStdDev())); lastAddedVisionTimestamp = visionTimestamp; - } + } poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition()); diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java new file mode 100644 index 0000000..9012ec8 --- /dev/null +++ b/src/main/java/frc/robot/vision/Limelight.java @@ -0,0 +1,30 @@ +package frc.robot.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import java.util.Optional; + +public class Limelight{ + private Optional rawVisionResult; + private Optional processedVisionResult; +public String limeLightName = ""; + + + public Optional getRawVisionResult() { + var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limeLightName); + + if (estimatePose == null) { + return Optional.empty(); + } + + if (estimatePose.tagCount == 0) { + return Optional.empty(); + } + + // This prevents pose estimator from having crazy poses if the Limelight loses power + if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) { + return Optional.empty(); + } + + return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds)); + } +} diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 074b748..1e6a5ec 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -3,51 +3,39 @@ import frc.robot.imu.ImuSubsystem; import frc.robot.util.scheduling.SubsystemPriority; import frc.robot.util.state_machines.StateMachine; -import java.util.Optional; +import java.util.List; +import java.util.ArrayList; public class VisionSubsystem extends StateMachine { private final ImuSubsystem imu; - private Optional rawVisionResult; - private Optional processedVisionResult; + private final Limelight leftLimelight; + private final Limelight rightLimelight; + private final List processedVisionResult = new ArrayList<>(); - public VisionSubsystem(ImuSubsystem imu) { + + public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight rightLimelight) { super(SubsystemPriority.VISION, VisionState.DEFAULT_STATE); this.imu = imu; + this.leftLimelight = leftLimelight; + this.rightLimelight = rightLimelight; } - private Optional getRawVisionResult() { - var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(""); - - if (estimatePose == null) { - return Optional.empty(); - } + @Override + protected void collectInputs() { + var leftResult = leftLimelight.getRawVisionResult(); + var rightResult = rightLimelight.getRawVisionResult(); + processedVisionResult.clear(); - if (estimatePose.tagCount == 0) { - return Optional.empty(); + if (leftResult.isPresent()) { + processedVisionResult.add(leftResult.get()); } - - // This prevents pose estimator from having crazy poses if the Limelight loses power - if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) { - return Optional.empty(); + if(rightResult.isPresent()) { + processedVisionResult.add(rightResult.get()); } - return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds)); - } - - @Override - protected void collectInputs() { - rawVisionResult = getRawVisionResult(); - if (rawVisionResult.isEmpty()) { - processedVisionResult = Optional.empty(); - } else { - var rawData = rawVisionResult.get(); - processedVisionResult = - Optional.of( - new VisionResult(VisionUtil.interpolatePose(rawData.pose()), rawData.timestamp())); - } } - public Optional getVisionResult() { + public List getVisionResult() { return processedVisionResult; } From ccb70402d000bcaef88273624631db7d043e125a Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Tue, 17 Sep 2024 19:01:10 -0700 Subject: [PATCH 2/2] format --- src/main/java/frc/robot/Robot.java | 15 ++++++++++----- .../robot/localization/LocalizationSubsystem.java | 10 ++++------ .../java/frc/robot/swerve/SwerveSubsystem.java | 10 +++------- src/main/java/frc/robot/vision/Limelight.java | 6 ++---- .../java/frc/robot/vision/VisionSubsystem.java | 6 ++---- 5 files changed, 21 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2e23cd5..9794e08 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,11 +132,16 @@ public void testPeriodic() {} public void testExit() {} private void configureBindings() { - swerve.setDefaultCommand(swerve.run(() -> { - if (DriverStation.isTeleop()) { - swerve.driveTeleop(hardware.driverController.getLeftX(),hardware.driverController.getLeftY() , hardware.driverController.getRightX()); - } - })); + swerve.setDefaultCommand( + swerve.run( + () -> { + if (DriverStation.isTeleop()) { + swerve.driveTeleop( + hardware.driverController.getLeftX(), + hardware.driverController.getLeftY(), + hardware.driverController.getRightX()); + } + })); hardware .driverController diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index f113cee..76207b6 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -11,9 +11,8 @@ import frc.robot.util.state_machines.StateMachine; import frc.robot.vision.VisionResult; import frc.robot.vision.VisionSubsystem; -import java.util.Optional; -import java.util.List; import java.util.ArrayList; +import java.util.List; public class LocalizationSubsystem extends StateMachine { private final ImuSubsystem imu; @@ -43,8 +42,7 @@ public Pose2d getPose() { @Override public void robotPeriodic() { super.robotPeriodic(); -for (int resultnum : latestResult.size()) - results = latestResult.get(resultnum); + for (var results : latestResult) { Pose2d visionPose = results.pose(); double visionTimestamp = results.timestamp(); @@ -60,9 +58,9 @@ public void robotPeriodic() { RobotConfig.get().vision().xyStdDev(), RobotConfig.get().vision().thetaStdDev())); lastAddedVisionTimestamp = visionTimestamp; + } + poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition()); } - - poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition()); } } diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java index 3d75b27..7135c5f 100644 --- a/src/main/java/frc/robot/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/swerve/SwerveSubsystem.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.config.RobotConfig; import frc.robot.fms.FmsSubsystem; import frc.robot.util.ControllerHelpers; @@ -120,14 +119,11 @@ public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) { public void driveTeleop(double x, double y, double theta) { double leftY = -1.0 - * ControllerHelpers.getExponent( - ControllerHelpers.getDeadbanded(x, leftYDeadband), 1.5); + * ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(x, leftYDeadband), 1.5); double leftX = - ControllerHelpers.getExponent( - ControllerHelpers.getDeadbanded(y, leftXDeadband), 1.5); + ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(y, leftXDeadband), 1.5); double rightX = - ControllerHelpers.getExponent( - ControllerHelpers.getDeadbanded(theta, rightXDeadband), 2); + ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(theta, rightXDeadband), 2); if (RobotConfig.get().swerve().invertRotation()) { rightX *= -1.0; diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java index 9012ec8..a6905d8 100644 --- a/src/main/java/frc/robot/vision/Limelight.java +++ b/src/main/java/frc/robot/vision/Limelight.java @@ -1,13 +1,11 @@ package frc.robot.vision; -import edu.wpi.first.math.geometry.Pose2d; import java.util.Optional; -public class Limelight{ +public class Limelight { private Optional rawVisionResult; private Optional processedVisionResult; -public String limeLightName = ""; - + public String limeLightName = ""; public Optional getRawVisionResult() { var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limeLightName); diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 1e6a5ec..77b5708 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -3,8 +3,8 @@ import frc.robot.imu.ImuSubsystem; import frc.robot.util.scheduling.SubsystemPriority; import frc.robot.util.state_machines.StateMachine; -import java.util.List; import java.util.ArrayList; +import java.util.List; public class VisionSubsystem extends StateMachine { private final ImuSubsystem imu; @@ -12,7 +12,6 @@ public class VisionSubsystem extends StateMachine { private final Limelight rightLimelight; private final List processedVisionResult = new ArrayList<>(); - public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight rightLimelight) { super(SubsystemPriority.VISION, VisionState.DEFAULT_STATE); this.imu = imu; @@ -29,10 +28,9 @@ protected void collectInputs() { if (leftResult.isPresent()) { processedVisionResult.add(leftResult.get()); } - if(rightResult.isPresent()) { + if (rightResult.isPresent()) { processedVisionResult.add(rightResult.get()); } - } public List getVisionResult() {