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 4448e29..76207b6 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -11,7 +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.ArrayList; +import java.util.List; public class LocalizationSubsystem extends StateMachine { private final ImuSubsystem imu; @@ -20,7 +21,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 +42,7 @@ public Pose2d getPose() { @Override public void robotPeriodic() { super.robotPeriodic(); - - if (latestResult.isPresent()) { - var results = latestResult.get(); + for (var results : latestResult) { Pose2d visionPose = results.pose(); double visionTimestamp = results.timestamp(); @@ -60,8 +59,8 @@ public void robotPeriodic() { 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 new file mode 100644 index 0000000..a6905d8 --- /dev/null +++ b/src/main/java/frc/robot/vision/Limelight.java @@ -0,0 +1,28 @@ +package frc.robot.vision; + +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..77b5708 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -3,51 +3,37 @@ 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.ArrayList; +import java.util.List; 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; - } - - private Optional getRawVisionResult() { - var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(""); - - 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)); + this.leftLimelight = leftLimelight; + this.rightLimelight = rightLimelight; } @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())); + var leftResult = leftLimelight.getRawVisionResult(); + var rightResult = rightLimelight.getRawVisionResult(); + processedVisionResult.clear(); + + if (leftResult.isPresent()) { + processedVisionResult.add(leftResult.get()); + } + if (rightResult.isPresent()) { + processedVisionResult.add(rightResult.get()); } } - public Optional getVisionResult() { + public List getVisionResult() { return processedVisionResult; }