From 3fff065ce03953714dd67d6451aadbdf95f59bda Mon Sep 17 00:00:00 2001 From: Anonyma Date: Mon, 10 Jun 2024 12:38:09 -0700 Subject: [PATCH 01/23] Positions and RobotContainer skeleton --- src/main/deploy/pathplanner/navgrid.json | 1 + src/main/java/frc/robot/RobotContainer.java | 47 +++++- .../java/frc/robot/constants/Positions.java | 138 +++++++++--------- 3 files changed, 112 insertions(+), 74 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7064644..fe75fff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,13 +6,17 @@ import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.constants.Positions.PositionInitialization; import frc.robot.constants.Constants.ControllerConstants; import frc.robot.constants.Constants.ShuffleboardTabNames; @@ -37,6 +41,9 @@ public static RobotContainer getInstance() { } private final SendableChooser autoChooser; + // TODO : Move to SwerveSubsystem once it exists + private final SendableChooser positionChooser = new SendableChooser(); + private final ShuffleboardLayout layout = Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT).getLayout("SwerveSubsystem", BuiltInLayouts.kList); // Instance of the controllers used to drive the robot private CommandXboxController driverController; @@ -46,7 +53,7 @@ public static RobotContainer getInstance() { public RobotContainer() { this.driverController = new CommandXboxController(ControllerConstants.DRIVER_CONTROLLER_ID); this.operatorController = new CommandXboxController(ControllerConstants.OPERATOR_CONTROLLER_ID); - + initializeSubsystems(); // Register named commands for pathplanner (always do this after subsystem initialization) registerNamedCommands(); @@ -55,16 +62,37 @@ public RobotContainer() { configureOperatorBindings(); autoChooser = AutoBuilder.buildAutoChooser(); // Default auto will be Commands.none() - Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT) - .add("Auto Chooser", autoChooser) - .withWidget(BuiltInWidgets.kComboBoxChooser) - .withPosition(11, 0) - .withSize(4, 1); + // Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT) + // .add("Auto Chooser", autoChooser) + // .withWidget(BuiltInWidgets.kComboBoxChooser) + // .withPosition(11, 0) + // .withSize(4, 1); + SmartDashboard.putData("Auto Chooser", autoChooser); } /** Creates instances of each subsystem so periodic runs */ private void initializeSubsystems() { + // TODO : Move this to SwerveSubsystem once it's created + for (PositionInitialization position : PositionInitialization.values()) { + this.positionChooser.addOption(position.name(), position); + if (position == PositionInitialization.LIMELIGHT) { + this.positionChooser.setDefaultOption(position.name(), position); + } + } + + this.positionChooser.onChange((PositionInitialization position) -> {} + // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(position)) + ); + + this.layout.add("Starting Position", this.positionChooser) + .withWidget(BuiltInWidgets.kComboBoxChooser); + // Re-set chosen position. + this.layout.add("Set Starting Position", + Commands.runOnce(() -> {} + // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(this.positionChooser.getSelected())) + ).ignoringDisable(true).withName("Set Again")) + .withWidget(BuiltInWidgets.kCommand); } /** Register all NamedCommands for PathPlanner use */ @@ -83,8 +111,11 @@ private void configureDriverBindings() { /** Configures the button bindings of the driver controller */ private void configureOperatorBindings() { // Unused while building and testing new kraken swerve drive - // This line doesn't do anything, it just removes the "unused" syntax from the variable - this.operatorController.toString(); + + // Cancel all currently scheduled commands + operatorController.b().onTrue(Commands.runOnce(() -> { + CommandScheduler.getInstance().cancelAll(); + })); } /** diff --git a/src/main/java/frc/robot/constants/Positions.java b/src/main/java/frc/robot/constants/Positions.java index 5dff437..d3277a9 100644 --- a/src/main/java/frc/robot/constants/Positions.java +++ b/src/main/java/frc/robot/constants/Positions.java @@ -4,6 +4,7 @@ package frc.robot.constants; +import java.util.Optional; import java.util.Map; import edu.wpi.first.math.geometry.Pose2d; @@ -16,85 +17,90 @@ * A class that stores positions used for initialization and calculations. */ public final class Positions { + /** + * An enum used to decide which position to initialize to. + */ + public enum PositionInitialization { + TOP, + MIDDLE, + BOTTOM, + /** This position will be initialized to whatever the {@link VisionSubsystem} sees */ + LIMELIGHT + ; + } + // TODO : redo this file, because it's a freaking mess /** SPEAKER positions to target */ - public static final Map SPEAKER_TARGETS = Map.ofEntries( + private static final Map SPEAKER_TARGET = Map.ofEntries( Map.entry(DriverStation.Alliance.Blue, new Translation3d(0, 5.55, 3)), Map.entry(DriverStation.Alliance.Red, new Translation3d(16.5, 5.55, 3)) ); - /** Starting positions using the PathPlanner field as reference */ - public static enum StartingPositions { - TOP("Top", 3), - MIDDLE("Middle", 2), - BOTTOM("Bottom", 1), - AUTO("Auto", 0) - ; - - String name; - int location; - - private StartingPositions(String name, int position) { - this.name = name; - this.location = position; - } - /** - * Gets the human-readable name of the position. - * @return the name. - */ - public String getName() { - return this.name; - } - /** - * Gets the integer location based on {@link DriverStation#getLocation()}. - * @return the location. - */ - public int getLocation() { - return this.location; - } - /** - * Gets an array of the {@link StartingPositions}. - * @return the array. - */ - public static StartingPositions[] getStartingPositions() { - return new StartingPositions[]{TOP, MIDDLE, BOTTOM, AUTO}; - } - } + /** AMP positions to line up to */ + private static final Map AMP_TARGET = Map.ofEntries( + Map.entry(DriverStation.Alliance.Blue, + new Pose2d(new Translation2d(1.8, 7.66), Rotation2d.fromDegrees(90)) + ), + Map.entry(DriverStation.Alliance.Red, + new Pose2d(new Translation2d(14.7, 7.66), Rotation2d.fromDegrees(-90)) + ) + ); /** Initial bot positions used for initializing odometry, blue-alliance relative. */ - public static final Map> STARTING_POSITIONS = Map.ofEntries( + private static final Map> STARTING_POSITIONS = Map.ofEntries( Map.entry(DriverStation.Alliance.Blue, Map.ofEntries( - Map.entry(3, new Pose2d(new Translation2d(0.75, 6.66), Rotation2d.fromDegrees(60))), - Map.entry(2, new Pose2d(new Translation2d(1.34, 5.55), Rotation2d.fromDegrees(0))), - Map.entry(1, new Pose2d(new Translation2d(0.75, 4.45), Rotation2d.fromDegrees(300))))), + Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(0.75, 6.66), Rotation2d.fromDegrees(60))), + Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(1.34, 5.55), Rotation2d.fromDegrees(0))), + Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(0.75, 4.45), Rotation2d.fromDegrees(300))))), Map.entry(DriverStation.Alliance.Red, Map.ofEntries( - Map.entry(3, new Pose2d(new Translation2d(15.8, 6.66), Rotation2d.fromDegrees(300))), - Map.entry(2, new Pose2d(new Translation2d(15.2, 5.55), Rotation2d.fromDegrees(180))), - Map.entry(1, new Pose2d(new Translation2d(15.8, 4.50), Rotation2d.fromDegrees(60))))) + Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(15.8, 6.66), Rotation2d.fromDegrees(300))), + Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(15.2, 5.55), Rotation2d.fromDegrees(180))), + Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(15.8, 4.50), Rotation2d.fromDegrees(60))))) ); + + /** + * Get the starting position of the robot. + * @param position to get. + * @return a Pose2d. + */ + public static Pose2d getStartingPose(PositionInitialization position) { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty()) { + System.err.println("Starting Position : Alliance is empty ; relying on Limelight data."); + } + + if (alliance.isEmpty() || position == PositionInitialization.LIMELIGHT) { + return null; // TODO : VisionSubsystem + // return VisionSubsystem.getEstimatedPose(); + } - /** Names used to organize pathfinding positions in {@link Positions#PATHFIND_POSITIONS}. */ - public static enum PathfindingPosition { - SPEAKER_TOP, - SPEAKER_MIDDLE, - SPEAKER_BOTTOM, - AMP + return Positions.STARTING_POSITIONS.get(alliance.get()).get(position); } - /** Position the robot should line up to, blue-alliance relative. */ - public static final Map> PATHFIND_POSITIONS = Map.ofEntries( - Map.entry(DriverStation.Alliance.Blue, Map.ofEntries( - Map.entry(PathfindingPosition.SPEAKER_TOP, STARTING_POSITIONS.get(DriverStation.Alliance.Blue).get(StartingPositions.TOP.getLocation())), - Map.entry(PathfindingPosition.SPEAKER_MIDDLE, STARTING_POSITIONS.get(DriverStation.Alliance.Blue).get(StartingPositions.MIDDLE.getLocation())), - Map.entry(PathfindingPosition.SPEAKER_BOTTOM, STARTING_POSITIONS.get(DriverStation.Alliance.Blue).get(StartingPositions.BOTTOM.getLocation())), - Map.entry(PathfindingPosition.AMP, new Pose2d(new Translation2d(1.8, 7.66), Rotation2d.fromDegrees(90))) - )), - Map.entry(DriverStation.Alliance.Red, Map.ofEntries( - Map.entry(PathfindingPosition.SPEAKER_TOP, STARTING_POSITIONS.get(DriverStation.Alliance.Red).get(StartingPositions.TOP.getLocation())), - Map.entry(PathfindingPosition.SPEAKER_MIDDLE, STARTING_POSITIONS.get(DriverStation.Alliance.Red).get(StartingPositions.MIDDLE.getLocation())), - Map.entry(PathfindingPosition.SPEAKER_BOTTOM, STARTING_POSITIONS.get(DriverStation.Alliance.Red).get(StartingPositions.BOTTOM.getLocation())), - Map.entry(PathfindingPosition.AMP, new Pose2d(new Translation2d(14.7, 7.66), Rotation2d.fromDegrees(-90))) - )) - ); + /** + * Gets the SPEAKER target position for the current alliance. + * @throws RuntimeException if the alliance is empty. + * @return a Translation3d to aim for. + */ + public static Translation3d getSpeakerTarget() throws RuntimeException { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty()) { + throw new RuntimeException("Speaker Target : Alliance is empty ; cannot target SPEAKER."); + } + return Positions.SPEAKER_TARGET.get(alliance.get()); + } + + /** + * Gets the AMP target position for the current alliance. + * @throws RuntimeException if the alliance is empty. + * @return a Pose2d to drive to. + */ + public static Pose2d getAmpTarget() throws RuntimeException { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty()) { + throw new RuntimeException("Amp Target : Alliance is empty ; cannot target AMP."); + } + return Positions.AMP_TARGET.get(alliance.get()); + } } From 4276a59b18ffb0ffe8507cb6f116fb77e085b761 Mon Sep 17 00:00:00 2001 From: Anonyma Date: Mon, 10 Jun 2024 15:11:49 -0700 Subject: [PATCH 02/23] swerve --- src/main/java/frc/robot/Robot.java | 22 ++- src/main/java/frc/robot/RobotContainer.java | 82 ++++++++++- .../robot/constants/PhysicalConstants.java | 4 +- .../robot/swerve/CommandSwerveDrivetrain.java | 124 ++++++++++++++++ src/main/java/frc/robot/swerve/Telemetry.java | 115 +++++++++++++++ .../java/frc/robot/swerve/TunerConstants.java | 137 ++++++++++++++++++ 6 files changed, 477 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java create mode 100644 src/main/java/frc/robot/swerve/Telemetry.java create mode 100644 src/main/java/frc/robot/swerve/TunerConstants.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d851568..79c42b2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,6 +26,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + // TODO LL // Port forward all required LL ports. Necessary for robot connections over ethernet. // for (int port = 5800; port <= 5807; port++) { // PortForwarder.add(port, LimelightConstants.INTAKE_LLIGHT + ".local", port); @@ -49,6 +50,25 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + + // TODO implement LL using this stub + /** + * This example of adding Limelight is very simple and may not be sufficient for on-field use. + * Users typically need to provide a standard deviation that scales with the distance to target + * and changes with number of tags available. + * + * This example is sufficient to show that vision integration is possible, though exact implementation + * of how to use vision should be tuned per-robot and to the team's specification. + */ + // if (UseLimelight) { + // var lastResult = LimelightHelpers.getLatestResults("limelight").targetingResults; + + // Pose2d llPose = lastResult.getBotPose2d_wpiBlue(); + + // if (lastResult.valid) { + // m_robotContainer.drivetrain.addVisionMeasurement(llPose, Timer.getFPGATimestamp()); + // } + // } } @Override @@ -73,7 +93,7 @@ public void autonomousInit() { } } else { - System.err.println("No auton command found"); + System.err.println("No auton command found."); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe75fff..fbd6945 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,14 @@ package frc.robot; +import java.util.Map; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -17,6 +23,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.constants.Positions.PositionInitialization; +import frc.robot.swerve.CommandSwerveDrivetrain; +import frc.robot.swerve.Telemetry; +import frc.robot.swerve.TunerConstants; import frc.robot.constants.Constants.ControllerConstants; import frc.robot.constants.Constants.ShuffleboardTabNames; @@ -54,6 +63,8 @@ public RobotContainer() { this.driverController = new CommandXboxController(ControllerConstants.DRIVER_CONTROLLER_ID); this.operatorController = new CommandXboxController(ControllerConstants.OPERATOR_CONTROLLER_ID); + configureDrivetrain(); // This is done separately because it works differently from other Subsystems + initializeSubsystems(); // Register named commands for pathplanner (always do this after subsystem initialization) registerNamedCommands(); @@ -61,17 +72,80 @@ public RobotContainer() { configureDriverBindings(); configureOperatorBindings(); - autoChooser = AutoBuilder.buildAutoChooser(); // Default auto will be Commands.none() + this.autoChooser = AutoBuilder.buildAutoChooser(); // Default auto will be Commands.none() // Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT) // .add("Auto Chooser", autoChooser) // .withWidget(BuiltInWidgets.kComboBoxChooser) // .withPosition(11, 0) // .withSize(4, 1); - SmartDashboard.putData("Auto Chooser", autoChooser); + SmartDashboard.putData("Auto Chooser", this.autoChooser); + } + + /** + * This function initializes the swerve subsystem and configures its bindings with the driver controller. + * This is based on the {@code Phoenix6 Swerve Example} that can be found on GitHub. + */ + private void configureDrivetrain() { + final double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed + final double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity + final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain + + final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * ControllerConstants.DEADBAND).withRotationalDeadband(MaxAngularRate * ControllerConstants.DEADBAND) // Add a deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric driving in open loop + + final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + final SwerveRequest.FieldCentric fieldCentricMove = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + final Telemetry logger = new Telemetry(MaxSpeed); + + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + .ignoringDisable(true) + ); + + driverController.x().whileTrue(drivetrain.applyRequest(() -> brake)); + driverController.y().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection( + new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX())) + )); + + // Burger + driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); + // Double Rectangle + // TODO : Reset using LL data + driverController.back().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative(new Pose2d()))); + + // This looks terrible, but I can't think of a better way to do it povSpeeds = Map.ofEntries( + Map.entry( 0, new Double[]{ 0.5, 0.0}), + Map.entry( 45, new Double[]{ 0.5, 0.5}), + Map.entry( 90, new Double[]{ 0.0, 0.5}), + Map.entry(135, new Double[]{-0.5, 0.5}), + Map.entry(180, new Double[]{-0.5, 0.0}), + Map.entry(225, new Double[]{-0.5, -0.5}), + Map.entry(270, new Double[]{ 0.0, -0.5}), + Map.entry(315, new Double[]{ 0.5, -0.5}) + ); + + povSpeeds.forEach( + (Integer angle, Double[] speeds) -> driverController.pov(angle).whileTrue( + drivetrain.applyRequest(() -> fieldCentricMove.withVelocityX(speeds[0]).withVelocityY(speeds[1])) + ) + ); + } + + drivetrain.registerTelemetry(logger::telemeterize); } /** Creates instances of each subsystem so periodic runs */ - private void initializeSubsystems() { + private void initializeSubsystems() { // TODO : Move this to SwerveSubsystem once it's created for (PositionInitialization position : PositionInitialization.values()) { @@ -106,6 +180,8 @@ private void configureDriverBindings() { driverController.b().onTrue(Commands.runOnce(() -> { CommandScheduler.getInstance().cancelAll(); })); + + // POV, joysticks, and start/back are all used in configureDrivetrain() } /** Configures the button bindings of the driver controller */ diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index b30d1bf..527e989 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -14,9 +14,7 @@ public final class PhysicalConstants { */ public static final class RobotConstants { /** Name of the CTRE CAN bus. */ - public static final String CTRE_CAN_BUS = "ctre"; - /** CAN ID for the Pigeon2. */ - public static final int GYRO_ID = 1; + public static final String CTRE_CAN_BUS = "canivore"; } /** Constants for limelight-related data. */ diff --git a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..fdd203e --- /dev/null +++ b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java @@ -0,0 +1,124 @@ +package frc.robot.swerve; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * Class that extends the Phoenix SwerveDrivetrain class and implements + * subsystem so it can be used in command-based projects easily. + */ +public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean hasAppliedOperatorPerspective = false; + + private final SwerveRequest.ApplyChassisSpeeds AutoRequest = new SwerveRequest.ApplyChassisSpeeds(); + + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + configurePathPlanner(); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + private void configurePathPlanner() { + double driveBaseRadius = 0; + for (var moduleLocation : m_moduleLocations) { + driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); + } + + AutoBuilder.configureHolonomic( + () -> this.getState().Pose, // Supplier of current robot pose + this::seedFieldRelative, // Consumer for seeding pose against auto + this::getCurrentRobotChassisSpeeds, + (speeds) -> this.setControl(AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot + new HolonomicPathFollowerConfig( + new PIDConstants(10, 0, 0), + new PIDConstants(10, 0, 0), + TunerConstants.kSpeedAt12VoltsMps, + driveBaseRadius, + new ReplanningConfig() + ), + () -> DriverStation.getAlliance().orElse(Alliance.Blue)==Alliance.Red, // Assume the path needs to be flipped for Red vs Blue, this is normally the case + this); // Subsystem for requirements + } + + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + public Command getAutoPath(String pathName) { + return new PathPlannerAuto(pathName); + } + + public ChassisSpeeds getCurrentRobotChassisSpeeds() { + return m_kinematics.toChassisSpeeds(getState().ModuleStates); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + @Override + public void periodic() { + /* Periodically try to apply the operator perspective */ + /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ + /* This allows us to correct the perspective in case the robot code restarts mid-match */ + /* Otherwise, only check and apply the operator perspective if the DS is disabled */ + /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ + if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent((allianceColor) -> { + this.setOperatorPerspectiveForward( + allianceColor == Alliance.Red ? + RedAlliancePerspectiveRotation : BlueAlliancePerspectiveRotation); + hasAppliedOperatorPerspective = true; + }); + } + } +} diff --git a/src/main/java/frc/robot/swerve/Telemetry.java b/src/main/java/frc/robot/swerve/Telemetry.java new file mode 100644 index 0000000..c6e195f --- /dev/null +++ b/src/main/java/frc/robot/swerve/Telemetry.java @@ -0,0 +1,115 @@ +package frc.robot.swerve; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Robot speeds for general checking */ + private final NetworkTable driveStats = inst.getTable("Drive"); + private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + private final DoublePublisher odomFreq = driveStats.getDoubleTopic("Odometry Frequency").publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + private Pose2d m_lastPose = new Pose2d(); + private double lastTime = Utils.getCurrentTimeSeconds(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + /* Accept the swerve drive state and telemeterize it to smartdashboard */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the pose */ + Pose2d pose = state.Pose; + fieldTypePub.set("Field2d"); + fieldPub.set(new double[] { + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees() + }); + + /* Telemeterize the robot's general speeds */ + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); + m_lastPose = pose; + + Translation2d velocities = distanceDiff.div(diffTime); + + speed.set(velocities.getNorm()); + velocityX.set(velocities.getX()); + velocityY.set(velocities.getY()); + odomFreq.set(1.0 / state.OdometryPeriod); + + /* Telemeterize the module's states */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + + SignalLogger.writeDoubleArray("odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); + } +} diff --git a/src/main/java/frc/robot/swerve/TunerConstants.java b/src/main/java/frc/robot/swerve/TunerConstants.java new file mode 100644 index 0000000..8302b66 --- /dev/null +++ b/src/main/java/frc/robot/swerve/TunerConstants.java @@ -0,0 +1,137 @@ +package frc.robot.swerve; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; + +import edu.wpi.first.math.util.Units; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.2) + .withKS(0).withKV(1.5).withKA(0); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(3).withKI(0).withKD(0) + .withKS(0).withKV(0).withKA(0); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrentA = 300.0; + + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12VoltsMps = 4.73; + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.746031746031747; + private static final double kSteerGearRatio = 21.428571428571427; + private static final double kWheelRadiusInches = 2; + + private static final boolean kSteerMotorReversed = true; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final String kCANbusName = "canivore"; + private static final int kPigeonId = 13; + + + // These are only used for simulation + private static final double kSteerInertia = 0.00001; + private static final double kDriveInertia = 0.001; + // Simulated voltage necessary to overcome friction + private static final double kSteerFrictionVoltage = 0.25; + private static final double kDriveFrictionVoltage = 0.25; + + private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(kPigeonId) + .withCANbusName(kCANbusName); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withSteerMotorInverted(kSteerMotorReversed); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 6; + private static final int kFrontLeftSteerMotorId = 5; + private static final int kFrontLeftEncoderId = 1; + private static final double kFrontLeftEncoderOffset = 0.144775390625; + + private static final double kFrontLeftXPosInches = 6.25; + private static final double kFrontLeftYPosInches = 11; + + // Front Right + private static final int kFrontRightDriveMotorId = 8; + private static final int kFrontRightSteerMotorId = 7; + private static final int kFrontRightEncoderId = 2; + private static final double kFrontRightEncoderOffset = -0.408203125; + + private static final double kFrontRightXPosInches = 6.25; + private static final double kFrontRightYPosInches = -11; + + // Back Left + private static final int kBackLeftDriveMotorId = 12; + private static final int kBackLeftSteerMotorId = 11; + private static final int kBackLeftEncoderId = 3; + private static final double kBackLeftEncoderOffset = 0.01953125; + + private static final double kBackLeftXPosInches = -6.25; + private static final double kBackLeftYPosInches = 11; + + // Back Right + private static final int kBackRightDriveMotorId = 10; + private static final int kBackRightSteerMotorId = 9; + private static final int kBackRightEncoderId = 4; + private static final double kBackRightEncoderOffset = 0.097412109375; + + private static final double kBackRightXPosInches = -6.25; + private static final double kBackRightYPosInches = -11; + + + private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide); + private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide); + private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide); + private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide); + + public static final CommandSwerveDrivetrain DriveTrain = new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, + FrontRight, BackLeft, BackRight); +} From 3fde3c87ca747f9c6a7a944ce3a78ffb9cd321ec Mon Sep 17 00:00:00 2001 From: Arrowbotics Date: Mon, 10 Jun 2024 18:14:15 -0700 Subject: [PATCH 03/23] Renamed CAN, updated bindings --- src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++--------- .../robot/constants/PhysicalConstants.java | 2 +- .../java/frc/robot/swerve/TunerConstants.java | 4 +-- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbd6945..7f34fdb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,8 +111,11 @@ private void configureDrivetrain() { driverController.x().whileTrue(drivetrain.applyRequest(() -> brake)); driverController.y().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection( - new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX())) - )); + new Rotation2d( + Math.abs(driverController.getLeftY()) >= 0.25 ? -driverController.getLeftY() : 0, + Math.abs(driverController.getLeftX()) >= 0.25 ? -driverController.getLeftX() : 0 + ) + ))); // Burger driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); @@ -124,14 +127,14 @@ private void configureDrivetrain() { if (ControllerConstants.DPAD_DRIVE_INPUT) { /** POV angle : [X velocity, Y velocity] in m/s */ final Map povSpeeds = Map.ofEntries( - Map.entry( 0, new Double[]{ 0.5, 0.0}), - Map.entry( 45, new Double[]{ 0.5, 0.5}), - Map.entry( 90, new Double[]{ 0.0, 0.5}), - Map.entry(135, new Double[]{-0.5, 0.5}), - Map.entry(180, new Double[]{-0.5, 0.0}), - Map.entry(225, new Double[]{-0.5, -0.5}), - Map.entry(270, new Double[]{ 0.0, -0.5}), - Map.entry(315, new Double[]{ 0.5, -0.5}) + Map.entry( 0, new Double[]{ 0.25, 0.0}), + Map.entry( 45, new Double[]{ 0.25, -0.25}), + Map.entry( 90, new Double[]{ 0.0, -0.25}), + Map.entry(135, new Double[]{-0.25, -0.25}), + Map.entry(180, new Double[]{-0.25, 0.0}), + Map.entry(225, new Double[]{-0.25, 0.25}), + Map.entry(270, new Double[]{ 0.0, 0.25}), + Map.entry(315, new Double[]{ 0.25, 0.25}) ); povSpeeds.forEach( @@ -189,9 +192,9 @@ private void configureOperatorBindings() { // Unused while building and testing new kraken swerve drive // Cancel all currently scheduled commands - operatorController.b().onTrue(Commands.runOnce(() -> { - CommandScheduler.getInstance().cancelAll(); - })); + // operatorController.b().onTrue(Commands.runOnce(() -> { + // CommandScheduler.getInstance().cancelAll(); + // })); } /** diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index 527e989..a2212ac 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -14,7 +14,7 @@ public final class PhysicalConstants { */ public static final class RobotConstants { /** Name of the CTRE CAN bus. */ - public static final String CTRE_CAN_BUS = "canivore"; + public static final String CTRE_CAN_BUS = "ctre"; } /** Constants for limelight-related data. */ diff --git a/src/main/java/frc/robot/swerve/TunerConstants.java b/src/main/java/frc/robot/swerve/TunerConstants.java index 8302b66..ee95ab6 100644 --- a/src/main/java/frc/robot/swerve/TunerConstants.java +++ b/src/main/java/frc/robot/swerve/TunerConstants.java @@ -52,7 +52,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final String kCANbusName = "canivore"; + private static final String kCANbusName = "ctre"; private static final int kPigeonId = 13; @@ -81,7 +81,7 @@ public class TunerConstants { .withDriveInertia(kDriveInertia) .withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveFrictionVoltage(kDriveFrictionVoltage) - .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withFeedbackSource(SteerFeedbackType.RemoteCANcoder) .withCouplingGearRatio(kCoupleRatio) .withSteerMotorInverted(kSteerMotorReversed); From 95403dc74530e572f303c65c0095191ba238a2e7 Mon Sep 17 00:00:00 2001 From: Arrowbotics Date: Tue, 11 Jun 2024 15:49:48 -0700 Subject: [PATCH 04/23] TODOs, tested swerve. --- .pathplanner/settings.json | 12 ++++ src/main/java/frc/robot/RobotContainer.java | 56 +++++++++++-------- .../java/frc/robot/constants/Constants.java | 2 + .../robot/swerve/CommandSwerveDrivetrain.java | 4 +- src/main/java/frc/robot/swerve/Telemetry.java | 11 +++- .../java/frc/robot/swerve/TunerConstants.java | 14 +++-- 6 files changed, 67 insertions(+), 32 deletions(-) create mode 100644 .pathplanner/settings.json diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..5039214 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.74, + "robotLength": 0.74, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 4.73, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.73 +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f34fdb..7de926f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,10 +10,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -22,10 +20,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.Positions.PositionInitialization; import frc.robot.swerve.CommandSwerveDrivetrain; import frc.robot.swerve.Telemetry; import frc.robot.swerve.TunerConstants; +import frc.robot.constants.Positions; import frc.robot.constants.Constants.ControllerConstants; import frc.robot.constants.Constants.ShuffleboardTabNames; @@ -100,11 +100,17 @@ private void configureDrivetrain() { final Telemetry logger = new Telemetry(MaxSpeed); + Trigger rightBumper = driverController.rightBumper(); + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + drive + // Drive forward with negative Y (forward) + .withVelocityX(-driverController.getLeftY() * MaxSpeed * (rightBumper.getAsBoolean() ? ControllerConstants.FINE_CONTROL_MULT : 1)) + // Drive left with negative X (left) + .withVelocityY(-driverController.getLeftX() * MaxSpeed * (rightBumper.getAsBoolean() ? ControllerConstants.FINE_CONTROL_MULT : 1)) + // Drive counterclockwise with negative X (left + .withRotationalRate(-driverController.getRightX() * MaxAngularRate * (rightBumper.getAsBoolean() ? ControllerConstants.FINE_CONTROL_MULT : 1)) ) .ignoringDisable(true) ); @@ -121,7 +127,9 @@ private void configureDrivetrain() { driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); // Double Rectangle // TODO : Reset using LL data - driverController.back().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative(new Pose2d()))); + driverController.back().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative( + Positions.getStartingPose(PositionInitialization.MIDDLE) + ))); // This looks terrible, but I can't think of a better way to do it {} - // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(position)) - ); - - this.layout.add("Starting Position", this.positionChooser) - .withWidget(BuiltInWidgets.kComboBoxChooser); - // Re-set chosen position. - this.layout.add("Set Starting Position", - Commands.runOnce(() -> {} - // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(this.positionChooser.getSelected())) - ).ignoringDisable(true).withName("Set Again")) - .withWidget(BuiltInWidgets.kCommand); + // this.positionChooser.onChange((PositionInitialization position) -> {} + // // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(position)) + // ); + + // this.layout.add("Starting Position", this.positionChooser) + // .withWidget(BuiltInWidgets.kComboBoxChooser); + // // Re-set chosen position. + // this.layout.add("Set Starting Position", + // Commands.runOnce(() -> {} + // // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(this.positionChooser.getSelected())) + // ).ignoringDisable(true).withName("Set Again")) + // .withWidget(BuiltInWidgets.kCommand); } /** Register all NamedCommands for PathPlanner use */ diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 36d19e2..0dbc01f 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -26,5 +26,7 @@ public static final class ControllerConstants { public static final double DEADBAND = 0.075; /** Whether or not to accept directional pad input for movement. */ public static final boolean DPAD_DRIVE_INPUT = true; + /** Speed multiplier when using fine-control movement. */ + public static final double FINE_CONTROL_MULT = 0.15; } } diff --git a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java index fdd203e..9babe05 100644 --- a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java @@ -68,13 +68,13 @@ private void configurePathPlanner() { this::getCurrentRobotChassisSpeeds, (speeds) -> this.setControl(AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot new HolonomicPathFollowerConfig( - new PIDConstants(10, 0, 0), + new PIDConstants(10, 0, 0), // TODO : Tune PP new PIDConstants(10, 0, 0), TunerConstants.kSpeedAt12VoltsMps, driveBaseRadius, new ReplanningConfig() ), - () -> DriverStation.getAlliance().orElse(Alliance.Blue)==Alliance.Red, // Assume the path needs to be flipped for Red vs Blue, this is normally the case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, // Assume the path needs to be flipped for Red vs Blue, this is normally the case this); // Subsystem for requirements } diff --git a/src/main/java/frc/robot/swerve/Telemetry.java b/src/main/java/frc/robot/swerve/Telemetry.java index c6e195f..717eaa6 100644 --- a/src/main/java/frc/robot/swerve/Telemetry.java +++ b/src/main/java/frc/robot/swerve/Telemetry.java @@ -35,7 +35,8 @@ public Telemetry(double maxSpeed) { /* Robot pose for field positioning */ private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final DoubleArrayPublisher fieldPubDeg = table.getDoubleArrayTopic("robotPoseDeg").publish(); + private final DoubleArrayPublisher fieldPubRad = table.getDoubleArrayTopic("robotPoseRad").publish(); private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); /* Robot speeds for general checking */ @@ -80,11 +81,17 @@ public void telemeterize(SwerveDriveState state) { /* Telemeterize the pose */ Pose2d pose = state.Pose; fieldTypePub.set("Field2d"); - fieldPub.set(new double[] { + fieldPubDeg.set(new double[] { pose.getX(), pose.getY(), pose.getRotation().getDegrees() }); + // Use this one in AdvantageScope + fieldPubRad.set(new double[] { + pose.getX(), + pose.getY(), + pose.getRotation().getRadians() + }); /* Telemeterize the robot's general speeds */ double currentTime = Utils.getCurrentTimeSeconds(); diff --git a/src/main/java/frc/robot/swerve/TunerConstants.java b/src/main/java/frc/robot/swerve/TunerConstants.java index ee95ab6..1c1099b 100644 --- a/src/main/java/frc/robot/swerve/TunerConstants.java +++ b/src/main/java/frc/robot/swerve/TunerConstants.java @@ -1,5 +1,7 @@ package frc.robot.swerve; +import com.ctre.phoenix6.configs.MountPoseConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -12,7 +14,7 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. + // Both sets of gains need to be tuned to your individual robot. // TODO : Tune when the bot is done // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput @@ -34,10 +36,10 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final double kSlipCurrentA = 300.0; + private static final double kSlipCurrentA = 300; // 20.66 // Theoretical free speed (m/s) at 12v applied output; - // This needs to be tuned to your individual robot + // This needs to be tuned to your individual robot // TODO : Tune when the bot is done public static final double kSpeedAt12VoltsMps = 4.73; // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; @@ -63,9 +65,13 @@ public class TunerConstants { private static final double kSteerFrictionVoltage = 0.25; private static final double kDriveFrictionVoltage = 0.25; + private static final Pigeon2Configuration kPigeonConfigs = new Pigeon2Configuration() + .withMountPose(new MountPoseConfigs().withMountPoseYaw(0)); + private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withPigeon2Id(kPigeonId) - .withCANbusName(kCANbusName); + .withCANbusName(kCANbusName) + .withPigeon2Configs(kPigeonConfigs); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() .withDriveMotorGearRatio(kDriveGearRatio) From 39681c57634df7700d3b8fb52e550ae0b6f568b0 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Wed, 17 Jul 2024 15:27:52 -0700 Subject: [PATCH 05/23] Bump WPILib version to 2024.3.2 --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index b63c234..462fb1d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { From 6cc4efd4a6e8c4a07bc35408a6e13b1172a12f0f Mon Sep 17 00:00:00 2001 From: Anonyma Date: Fri, 19 Jul 2024 01:48:02 -0700 Subject: [PATCH 06/23] Tentative vision --- src/main/java/frc/robot/Robot.java | 31 +- src/main/java/frc/robot/RobotContainer.java | 54 +- .../robot/constants/PhysicalConstants.java | 6 + .../java/frc/robot/constants/Positions.java | 8 +- .../frc/robot/vision/LimelightHelpers.java | 1270 +++++++++++++++++ .../frc/robot/vision/VisionSubsystem.java | 143 ++ 6 files changed, 1458 insertions(+), 54 deletions(-) create mode 100644 src/main/java/frc/robot/vision/LimelightHelpers.java create mode 100644 src/main/java/frc/robot/vision/VisionSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 79c42b2..7282ef5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,11 @@ package frc.robot; +import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.PhysicalConstants.LimelightConstants; /** * The VM is configured to automatically run this class, and to call the @@ -26,12 +28,12 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // TODO LL // Port forward all required LL ports. Necessary for robot connections over ethernet. - // for (int port = 5800; port <= 5807; port++) { - // PortForwarder.add(port, LimelightConstants.INTAKE_LLIGHT + ".local", port); - // PortForwarder.add(port + 10, LimelightConstants.SHOOTER_LLIGHT + ".local", port); - // } + for (int port = 5800; port <= 5809; port++) { + PortForwarder.add(port, LimelightConstants.FRONT_APRIL_TAG_LL + ".local", port); + PortForwarder.add(port + 10, LimelightConstants.NOTE_DETECTION_LL + ".local", port); + PortForwarder.add(port + 20, LimelightConstants.BACK_APRIL_TAG_LL + ".local", port); + } // Initialize RobotContainer and all subsystems RobotContainer.getInstance(); @@ -50,25 +52,6 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - - // TODO implement LL using this stub - /** - * This example of adding Limelight is very simple and may not be sufficient for on-field use. - * Users typically need to provide a standard deviation that scales with the distance to target - * and changes with number of tags available. - * - * This example is sufficient to show that vision integration is possible, though exact implementation - * of how to use vision should be tuned per-robot and to the team's specification. - */ - // if (UseLimelight) { - // var lastResult = LimelightHelpers.getLatestResults("limelight").targetingResults; - - // Pose2d llPose = lastResult.getBotPose2d_wpiBlue(); - - // if (lastResult.valid) { - // m_robotContainer.drivetrain.addVisionMeasurement(llPose, Timer.getFPGATimestamp()); - // } - // } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7de926f..2840faf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -25,9 +26,10 @@ import frc.robot.swerve.CommandSwerveDrivetrain; import frc.robot.swerve.Telemetry; import frc.robot.swerve.TunerConstants; -import frc.robot.constants.Positions; +import frc.robot.vision.VisionSubsystem; import frc.robot.constants.Constants.ControllerConstants; import frc.robot.constants.Constants.ShuffleboardTabNames; +import frc.robot.constants.Positions; public class RobotContainer { // Thread-safe singleton design pattern. @@ -50,7 +52,7 @@ public static RobotContainer getInstance() { } private final SendableChooser autoChooser; - // TODO : Move to SwerveSubsystem once it exists + // Position chooser private final SendableChooser positionChooser = new SendableChooser(); private final ShuffleboardLayout layout = Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT).getLayout("SwerveSubsystem", BuiltInLayouts.kList); @@ -126,9 +128,8 @@ private void configureDrivetrain() { // Burger driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); // Double Rectangle - // TODO : Reset using LL data driverController.back().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative( - Positions.getStartingPose(PositionInitialization.MIDDLE) + VisionSubsystem.getInstance().getEstimatedPose() ))); // This looks terrible, but I can't think of a better way to do it + drivetrain.seedFieldRelative(Positions.getStartingPose(position)) + ); + + this.layout.add("Starting Position", this.positionChooser) + .withWidget(BuiltInWidgets.kComboBoxChooser); + // Re-set chosen position. + this.layout.add("Set Starting Position", + Commands.runOnce( + () -> drivetrain.seedFieldRelative(Positions.getStartingPose(this.positionChooser.getSelected())) + ).ignoringDisable(true).withName("Set Again")) + .withWidget(BuiltInWidgets.kCommand); } /** Creates instances of each subsystem so periodic runs */ - private void initializeSubsystems() { - // TODO : Move this to SwerveSubsystem once it's created - - // for (PositionInitialization position : PositionInitialization.values()) { - // this.positionChooser.addOption(position.name(), position); - // if (position == PositionInitialization.LIMELIGHT) { - // this.positionChooser.setDefaultOption(position.name(), position); - // } - // } - - // this.positionChooser.onChange((PositionInitialization position) -> {} - // // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(position)) - // ); - - // this.layout.add("Starting Position", this.positionChooser) - // .withWidget(BuiltInWidgets.kComboBoxChooser); - // // Re-set chosen position. - // this.layout.add("Set Starting Position", - // Commands.runOnce(() -> {} - // // SwerveSubsystem.getInstance().setPose(Positions.getStartingPose(this.positionChooser.getSelected())) - // ).ignoringDisable(true).withName("Set Again")) - // .withWidget(BuiltInWidgets.kCommand); + private void initializeSubsystems() { + VisionSubsystem.getInstance(); } /** Register all NamedCommands for PathPlanner use */ diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index a2212ac..7ba71fe 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -21,5 +21,11 @@ public static final class RobotConstants { public static final class LimelightConstants { /** Spams "Bad LL 2D/3D Pose Data" when no data is coming from the NetworkTableInstance for LL. */ public static final boolean SPAM_BAD_DATA = true; + /** Front left Limelight (April Tags) */ + public static final String FRONT_APRIL_TAG_LL = "limelight-stheno"; + /** Front right Limelight (Note detection) */ + public static final String NOTE_DETECTION_LL = "limelight-medusa"; // TODO : configure detection limelight + /** Back Limelight (April Tags) */ + public static final String BACK_APRIL_TAG_LL = "limelight-euryale"; } } diff --git a/src/main/java/frc/robot/constants/Positions.java b/src/main/java/frc/robot/constants/Positions.java index d3277a9..2d57c23 100644 --- a/src/main/java/frc/robot/constants/Positions.java +++ b/src/main/java/frc/robot/constants/Positions.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.vision.VisionSubsystem; /** * A class that stores positions used for initialization and calculations. @@ -71,8 +72,7 @@ public static Pose2d getStartingPose(PositionInitialization position) { } if (alliance.isEmpty() || position == PositionInitialization.LIMELIGHT) { - return null; // TODO : VisionSubsystem - // return VisionSubsystem.getEstimatedPose(); + return VisionSubsystem.getInstance().getEstimatedPose(); } return Positions.STARTING_POSITIONS.get(alliance.get()).get(position); @@ -86,7 +86,7 @@ public static Pose2d getStartingPose(PositionInitialization position) { public static Translation3d getSpeakerTarget() throws RuntimeException { Optional alliance = DriverStation.getAlliance(); if (alliance.isEmpty()) { - throw new RuntimeException("Speaker Target : Alliance is empty ; cannot target SPEAKER."); + throw new RuntimeException("SPEAKER Target : Alliance is empty ; cannot target SPEAKER."); } return Positions.SPEAKER_TARGET.get(alliance.get()); } @@ -99,7 +99,7 @@ public static Translation3d getSpeakerTarget() throws RuntimeException { public static Pose2d getAmpTarget() throws RuntimeException { Optional alliance = DriverStation.getAlliance(); if (alliance.isEmpty()) { - throw new RuntimeException("Amp Target : Alliance is empty ; cannot target AMP."); + throw new RuntimeException("AMP Target : Alliance is empty ; cannot target AMP."); } return Positions.AMP_TARGET.get(alliance.get()); } diff --git a/src/main/java/frc/robot/vision/LimelightHelpers.java b/src/main/java/frc/robot/vision/LimelightHelpers.java new file mode 100644 index 0000000..3102d49 --- /dev/null +++ b/src/main/java/frc/robot/vision/LimelightHelpers.java @@ -0,0 +1,1270 @@ +//LimelightHelpers v1.9 (REQUIRES 2024.9.1) + +package frc.robot.vision; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.constants.PhysicalConstants.LimelightConstants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + /** + * Makes a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + if (LimelightConstants.SPAM_BAD_DATA) { + System.err.println("Bad LL 3D Pose Data!"); + } + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + if (LimelightConstants.SPAM_BAD_DATA) { + System.err.println("Bad LL 2D Pose Data!"); + } + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles. + * + * @param pose The Pose3d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles. + * + * @param pose The Pose2d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + @SuppressWarnings("unused") + private static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + @SuppressWarnings("deprecation") + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java new file mode 100644 index 0000000..7cacf75 --- /dev/null +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -0,0 +1,143 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.vision; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.PhysicalConstants.LimelightConstants; +import frc.robot.swerve.TunerConstants; + +public class VisionSubsystem extends SubsystemBase { + // Thread-safe singleton design pattern. + private static volatile VisionSubsystem instance; + private static Object mutex = new Object(); + + public static VisionSubsystem getInstance() { + VisionSubsystem result = instance; + + if (result == null) { + synchronized (mutex) { + result = instance; + if (result == null) { + instance = result = new VisionSubsystem(); + } + } + } + return instance; + } + + /** Creates a new ExampleSubsystem. */ + private VisionSubsystem() { + super("VisionSubsystem"); + } + + /** + * Gets the most trustworthy data similar to the periodic loop for this subsystem. + * @return Estimated Pose2d by the most accurate limelight (could be innacurate still) + */ + public Pose2d getEstimatedPose() { + Rotation2d botRotation = TunerConstants.DriveTrain.getState().Pose.getRotation(); + LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, botRotation.getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, botRotation.getDegrees(), 0, 0, 0, 0, 0); + + LimelightHelpers.PoseEstimate frontLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate backLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + + // If there is no data, ignore vision updates + if (frontLLData.tagCount == 0 && backLLData.tagCount == 0) { + return null; + } + + // More tags = more accurate + if (frontLLData.tagCount > backLLData.tagCount) { + return new Pose2d(frontLLData.pose.getTranslation(), botRotation); + } + else if (backLLData.tagCount > frontLLData.tagCount) { + return new Pose2d(backLLData.pose.getTranslation(), botRotation); + } + + // If it's way closer, then trust only that one. 80% is a heuteristic + if (frontLLData.avgTagDist < backLLData.avgTagDist * 0.8) { + return new Pose2d(backLLData.pose.getTranslation(), botRotation); + } + else if (backLLData.avgTagDist < frontLLData.avgTagDist * 0.8) { + return new Pose2d(frontLLData.pose.getTranslation(), botRotation); + } + + // Both have the same amount of tags, both are almost equadistant from tags + return new Pose2d( + frontLLData.pose.getTranslation().plus(backLLData.pose.getTranslation()).div(2), + botRotation + ); + } + + /** + * A helper method for updating odometry using {@link LimelightHelpers#PosteEstimate} data + * @param data - Used for updating odometry + */ + private void updateOdometry(LimelightHelpers.PoseEstimate... data) { + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); + + for (LimelightHelpers.PoseEstimate d : data) { + if (d.avgTagDist < 2) { + TunerConstants.DriveTrain.seedFieldRelative( + new Pose2d( + TunerConstants.DriveTrain.getState().Pose.getTranslation(), + d.pose.getRotation() + ) + ); + } + + TunerConstants.DriveTrain.addVisionMeasurement( + d.pose, + d.timestampSeconds + ); + } + } + + // This method will be called once per scheduler run + @Override + public void periodic() { + double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); + LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); + + LimelightHelpers.PoseEstimate frontLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate backLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + + double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; + // If our angular velocity is greater than 720 degrees per second or there is no data, ignore vision updates + if (Math.abs(Units.radiansToDegrees(angularVelocityRadians)) > 720 + || (frontLLData.tagCount == 0 && backLLData.tagCount == 0)) { + return; + } + + // More tags = more accurate + if (frontLLData.tagCount > backLLData.tagCount) { + updateOdometry(frontLLData); + return; + } + else if (backLLData.tagCount > frontLLData.tagCount) { + updateOdometry(backLLData); + return; + } + + // If it's way closer, then trust only that one. 80% is a heuteristic + if (frontLLData.avgTagDist < backLLData.avgTagDist * 0.8) { + updateOdometry(frontLLData); + return; + } + else if (backLLData.avgTagDist < frontLLData.avgTagDist * 0.8) { + updateOdometry(backLLData); + return; + } + + // Both have the same amount of tags, both are almost equadistant from tags + updateOdometry(frontLLData, backLLData); + } +} From 5a4485dc36647a9a3cf719fa2c8819b91b009503 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Fri, 19 Jul 2024 18:26:37 -0700 Subject: [PATCH 07/23] Handle null data & stub for rotation --- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../java/frc/robot/constants/Positions.java | 4 ++-- .../frc/robot/vision/LimelightHelpers.java | 1 - .../frc/robot/vision/VisionSubsystem.java | 23 +++++++++++-------- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2840faf..9352aa6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,7 +129,8 @@ private void configureDrivetrain() { driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); // Double Rectangle driverController.back().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative( - VisionSubsystem.getInstance().getEstimatedPose() + // VisionSubsystem.getInstance().getEstimatedPose() + Positions.getStartingPose(PositionInitialization.MIDDLE) ))); // This looks terrible, but I can't think of a better way to do it > STARTING_POSITIONS = Map.ofEntries( Map.entry(DriverStation.Alliance.Blue, Map.ofEntries( Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(0.75, 6.66), Rotation2d.fromDegrees(60))), - Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(1.34, 5.55), Rotation2d.fromDegrees(0))), + Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(1.34, 5.55), Rotation2d.fromDegrees(180))), Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(0.75, 4.45), Rotation2d.fromDegrees(300))))), Map.entry(DriverStation.Alliance.Red, Map.ofEntries( Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(15.8, 6.66), Rotation2d.fromDegrees(300))), - Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(15.2, 5.55), Rotation2d.fromDegrees(180))), + Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(15.2, 5.55), Rotation2d.fromDegrees(0))), Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(15.8, 4.50), Rotation2d.fromDegrees(60))))) ); diff --git a/src/main/java/frc/robot/vision/LimelightHelpers.java b/src/main/java/frc/robot/vision/LimelightHelpers.java index 3102d49..2d1daa2 100644 --- a/src/main/java/frc/robot/vision/LimelightHelpers.java +++ b/src/main/java/frc/robot/vision/LimelightHelpers.java @@ -749,7 +749,6 @@ public static String[] getLimelightNTStringArray(String tableName, String entryN } - @SuppressWarnings("deprecation") public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 7cacf75..f1fc915 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -49,15 +49,15 @@ public Pose2d getEstimatedPose() { LimelightHelpers.PoseEstimate backLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); // If there is no data, ignore vision updates - if (frontLLData.tagCount == 0 && backLLData.tagCount == 0) { + if ((frontLLData == null || frontLLData.tagCount == 0) && (backLLData == null || backLLData.tagCount == 0)) { return null; } // More tags = more accurate - if (frontLLData.tagCount > backLLData.tagCount) { + if (backLLData == null || frontLLData.tagCount > backLLData.tagCount) { return new Pose2d(frontLLData.pose.getTranslation(), botRotation); } - else if (backLLData.tagCount > frontLLData.tagCount) { + else if (frontLLData == null || backLLData.tagCount > frontLLData.tagCount) { return new Pose2d(backLLData.pose.getTranslation(), botRotation); } @@ -84,11 +84,14 @@ private void updateOdometry(LimelightHelpers.PoseEstimate... data) { TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); for (LimelightHelpers.PoseEstimate d : data) { - if (d.avgTagDist < 2) { + // TODO : optimize & use both LLs for this correction + double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; + if (d.avgTagDist < 3 && Units.radiansToDegrees(angularVelocityRadians) <= 180) { + System.out.printf("Avg tag distance : %f", d.avgTagDist); TunerConstants.DriveTrain.seedFieldRelative( new Pose2d( TunerConstants.DriveTrain.getState().Pose.getTranslation(), - d.pose.getRotation() + LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL).pose.getRotation() ) ); } @@ -111,18 +114,18 @@ public void periodic() { LimelightHelpers.PoseEstimate backLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; - // If our angular velocity is greater than 720 degrees per second or there is no data, ignore vision updates - if (Math.abs(Units.radiansToDegrees(angularVelocityRadians)) > 720 - || (frontLLData.tagCount == 0 && backLLData.tagCount == 0)) { + // If our angular velocity is greater than 360 degrees per second or there is no data, ignore vision updates + if (Math.abs(Units.radiansToDegrees(angularVelocityRadians)) > 360 + || ((frontLLData == null || frontLLData.tagCount == 0) && (backLLData == null || backLLData.tagCount == 0))) { return; } // More tags = more accurate - if (frontLLData.tagCount > backLLData.tagCount) { + if (backLLData == null || frontLLData.tagCount > backLLData.tagCount) { updateOdometry(frontLLData); return; } - else if (backLLData.tagCount > frontLLData.tagCount) { + else if (frontLLData == null || backLLData.tagCount > frontLLData.tagCount) { updateOdometry(backLLData); return; } From 029f4e35a879d039084e63fda21534bd1f81339a Mon Sep 17 00:00:00 2001 From: Anonyma Date: Fri, 19 Jul 2024 21:22:31 -0700 Subject: [PATCH 08/23] Redone LL data processing & Yaw control --- .../robot/constants/PhysicalConstants.java | 2 + .../java/frc/robot/vision/LimelightData.java | 34 ++++ .../frc/robot/vision/VisionSubsystem.java | 161 +++++++++--------- 3 files changed, 119 insertions(+), 78 deletions(-) create mode 100644 src/main/java/frc/robot/vision/LimelightData.java diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index 7ba71fe..8b40cda 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -21,6 +21,8 @@ public static final class RobotConstants { public static final class LimelightConstants { /** Spams "Bad LL 2D/3D Pose Data" when no data is coming from the NetworkTableInstance for LL. */ public static final boolean SPAM_BAD_DATA = true; + /** The distance within which to use Limelight data in meters. */ + public static final int TRUST_TAG_DISTANCE = 12; /** Front left Limelight (April Tags) */ public static final String FRONT_APRIL_TAG_LL = "limelight-stheno"; /** Front right Limelight (Note detection) */ diff --git a/src/main/java/frc/robot/vision/LimelightData.java b/src/main/java/frc/robot/vision/LimelightData.java new file mode 100644 index 0000000..b1409d6 --- /dev/null +++ b/src/main/java/frc/robot/vision/LimelightData.java @@ -0,0 +1,34 @@ +package frc.robot.vision; + +import edu.wpi.first.math.util.Units; +import frc.robot.swerve.TunerConstants; + +/** + * A helper class used for storing MegaTag and MegaTag2 data from a Limelight + * to avoid unnecessary calls to the NetworkTables API. + */ +public class LimelightData { + public LimelightHelpers.PoseEstimate MegaTag; + public LimelightHelpers.PoseEstimate MegaTag2; + + /** + * Creates a new LimelightData object. + * @param MegaTag data. + * @param MegaTag2 data. + */ + public LimelightData(LimelightHelpers.PoseEstimate MegaTag, LimelightHelpers.PoseEstimate MegaTag2) { + this.MegaTag = MegaTag; + this.MegaTag2 = MegaTag2; + } + + /** + * Checks if the average tag distance and bot's rotational velocity + * are reasonable for trusting rotation data. + * @return Whether rotation data can be trusted. + */ + public boolean canTrustRotation() { + double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; + // 3 Meters + return this.MegaTag2.avgTagDist < 3 && Units.radiansToDegrees(angularVelocityRadians) <= 180; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index f1fc915..06ebc35 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.PhysicalConstants.LimelightConstants; @@ -37,68 +36,87 @@ private VisionSubsystem() { } /** - * Gets the most trustworthy data similar to the periodic loop for this subsystem. - * @return Estimated Pose2d by the most accurate limelight (could be innacurate still) + * Gets the most trustworthy data from each Limelight. + * @return LimelightData for each trustworthy Limelight data. */ - public Pose2d getEstimatedPose() { - Rotation2d botRotation = TunerConstants.DriveTrain.getState().Pose.getRotation(); - LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, botRotation.getDegrees(), 0, 0, 0, 0, 0); - LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, botRotation.getDegrees(), 0, 0, 0, 0, 0); + private LimelightData[] getLimelightData() { + double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); + LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); - LimelightHelpers.PoseEstimate frontLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate backLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate frontLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); - // If there is no data, ignore vision updates - if ((frontLLData == null || frontLLData.tagCount == 0) && (backLLData == null || backLLData.tagCount == 0)) { - return null; + double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; + // If our angular velocity is greater than 360 degrees per second or there is no data, ignore vision updates + if (Math.abs(Units.radiansToDegrees(angularVelocityRadians)) > 360 + || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) + && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) + || (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { + return new LimelightData[0]; } // More tags = more accurate - if (backLLData == null || frontLLData.tagCount > backLLData.tagCount) { - return new Pose2d(frontLLData.pose.getTranslation(), botRotation); + if (backLLDataMT2 == null + || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + || frontLLDataMT2.tagCount > backLLDataMT2.tagCount) { + return new LimelightData[]{ new LimelightData(frontLLDataMT, frontLLDataMT2) }; } - else if (frontLLData == null || backLLData.tagCount > frontLLData.tagCount) { - return new Pose2d(backLLData.pose.getTranslation(), botRotation); + else if (frontLLDataMT2 == null + || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + || backLLDataMT2.tagCount > frontLLDataMT2.tagCount) { + return new LimelightData[]{ new LimelightData(backLLDataMT, backLLDataMT2) }; } // If it's way closer, then trust only that one. 80% is a heuteristic - if (frontLLData.avgTagDist < backLLData.avgTagDist * 0.8) { - return new Pose2d(backLLData.pose.getTranslation(), botRotation); + if (frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.8) { + return new LimelightData[]{ new LimelightData(frontLLDataMT, frontLLDataMT2) }; } - else if (backLLData.avgTagDist < frontLLData.avgTagDist * 0.8) { - return new Pose2d(frontLLData.pose.getTranslation(), botRotation); + else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { + return new LimelightData[]{ new LimelightData(backLLDataMT, backLLDataMT2) }; } // Both have the same amount of tags, both are almost equadistant from tags - return new Pose2d( - frontLLData.pose.getTranslation().plus(backLLData.pose.getTranslation()).div(2), - botRotation - ); + return new LimelightData[]{ + new LimelightData(frontLLDataMT, frontLLDataMT2), + new LimelightData(backLLDataMT, backLLDataMT2) + }; } /** - * A helper method for updating odometry using {@link LimelightHelpers#PosteEstimate} data - * @param data - Used for updating odometry + * Gets the most trustworthy data from each Limelight and returns a {@link Pose2d} object. + * @return The most accurate {@link Pose2d}, or {@code null} if there is none. */ - private void updateOdometry(LimelightHelpers.PoseEstimate... data) { - TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); - - for (LimelightHelpers.PoseEstimate d : data) { - // TODO : optimize & use both LLs for this correction - double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; - if (d.avgTagDist < 3 && Units.radiansToDegrees(angularVelocityRadians) <= 180) { - System.out.printf("Avg tag distance : %f", d.avgTagDist); - TunerConstants.DriveTrain.seedFieldRelative( - new Pose2d( - TunerConstants.DriveTrain.getState().Pose.getTranslation(), - LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL).pose.getRotation() - ) - ); - } + public Pose2d getEstimatedPose() { + LimelightData[] limelightDatas = getLimelightData(); - TunerConstants.DriveTrain.addVisionMeasurement( - d.pose, - d.timestampSeconds + if (limelightDatas.length == 0) { + return null; + } + else if (limelightDatas.length == 1) { + return new Pose2d( + limelightDatas[0].MegaTag2.pose.getTranslation(), + limelightDatas[0].canTrustRotation() ? + limelightDatas[0].MegaTag.pose.getRotation() : TunerConstants.DriveTrain.getState().Pose.getRotation() + ); + } + else { + // Average them for best accuracy + return new Pose2d( + // (First translation + Second translation) / 2 + limelightDatas[0].MegaTag2.pose.getTranslation().plus(limelightDatas[1].MegaTag2.pose.getTranslation()).div(2), + limelightDatas[0].canTrustRotation() ? + // First rotation / 2 + Second rotation / 2 + /* + * This is done to avoid problems due to Rotation2d being [0, 360) + * Ex : 180+180=0 followed by 0/2=0 when it should be 180+180=360 and 360/2=180. + */ + limelightDatas[0].MegaTag.pose.getRotation().div(2) + .plus(limelightDatas[1].MegaTag.pose.getRotation().div(2)) : + TunerConstants.DriveTrain.getState().Pose.getRotation() ); } } @@ -106,41 +124,28 @@ private void updateOdometry(LimelightHelpers.PoseEstimate... data) { // This method will be called once per scheduler run @Override public void periodic() { - double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); - LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); - LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); - - LimelightHelpers.PoseEstimate frontLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate backLLData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); - - double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; - // If our angular velocity is greater than 360 degrees per second or there is no data, ignore vision updates - if (Math.abs(Units.radiansToDegrees(angularVelocityRadians)) > 360 - || ((frontLLData == null || frontLLData.tagCount == 0) && (backLLData == null || backLLData.tagCount == 0))) { - return; - } - - // More tags = more accurate - if (backLLData == null || frontLLData.tagCount > backLLData.tagCount) { - updateOdometry(frontLLData); - return; - } - else if (frontLLData == null || backLLData.tagCount > frontLLData.tagCount) { - updateOdometry(backLLData); - return; - } + LimelightData[] limelightDatas = getLimelightData(); + + for (LimelightData data : limelightDatas) { + if (data.canTrustRotation()) { + // Only trust rotational data + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 0.7)); + TunerConstants.DriveTrain.addVisionMeasurement( + data.MegaTag.pose, + data.MegaTag.timestampSeconds + ); + } - // If it's way closer, then trust only that one. 80% is a heuteristic - if (frontLLData.avgTagDist < backLLData.avgTagDist * 0.8) { - updateOdometry(frontLLData); - return; - } - else if (backLLData.avgTagDist < frontLLData.avgTagDist * 0.8) { - updateOdometry(backLLData); - return; + // Only trust positional data + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); + TunerConstants.DriveTrain.addVisionMeasurement( + data.MegaTag2.pose, + data.MegaTag2.timestampSeconds + ); } - - // Both have the same amount of tags, both are almost equadistant from tags - updateOdometry(frontLLData, backLLData); } } + +// TODO : Smart cropping ? +// TODO : Pipeline switching ? (downscaling when closer to tags, cancelling when losing a tag) +// TODO : Tag filtering ? (similar to smart cropping & pipeline switching) \ No newline at end of file From fe4d26e592624a1905ba4d1167d40346d5eab2ee Mon Sep 17 00:00:00 2001 From: Anonyma Date: Sat, 20 Jul 2024 00:40:14 -0700 Subject: [PATCH 09/23] Fixed starting position rotations --- src/main/java/frc/robot/constants/Positions.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/Positions.java b/src/main/java/frc/robot/constants/Positions.java index 60d7d10..0662e86 100644 --- a/src/main/java/frc/robot/constants/Positions.java +++ b/src/main/java/frc/robot/constants/Positions.java @@ -30,8 +30,6 @@ public enum PositionInitialization { ; } - // TODO : redo this file, because it's a freaking mess - /** SPEAKER positions to target */ private static final Map SPEAKER_TARGET = Map.ofEntries( Map.entry(DriverStation.Alliance.Blue, new Translation3d(0, 5.55, 3)), @@ -51,15 +49,15 @@ public enum PositionInitialization { /** Initial bot positions used for initializing odometry, blue-alliance relative. */ private static final Map> STARTING_POSITIONS = Map.ofEntries( Map.entry(DriverStation.Alliance.Blue, Map.ofEntries( - Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(0.75, 6.66), Rotation2d.fromDegrees(60))), + Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(0.75, 6.66), Rotation2d.fromDegrees(240))), Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(1.34, 5.55), Rotation2d.fromDegrees(180))), - Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(0.75, 4.45), Rotation2d.fromDegrees(300))))), + Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(0.75, 4.45), Rotation2d.fromDegrees(120))))), Map.entry(DriverStation.Alliance.Red, Map.ofEntries( Map.entry(PositionInitialization.TOP, new Pose2d(new Translation2d(15.8, 6.66), Rotation2d.fromDegrees(300))), Map.entry(PositionInitialization.MIDDLE, new Pose2d(new Translation2d(15.2, 5.55), Rotation2d.fromDegrees(0))), Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(15.8, 4.50), Rotation2d.fromDegrees(60))))) ); - + /** * Get the starting position of the robot. * @param position to get. From be68f0b06d6a1088db63edb7074e1c6ef071195f Mon Sep 17 00:00:00 2001 From: Anonyma Date: Sat, 20 Jul 2024 02:22:23 -0700 Subject: [PATCH 10/23] Pipeline switching for downscaling (needs to be tuned) --- .../robot/constants/PhysicalConstants.java | 7 +++ .../java/frc/robot/vision/LimelightData.java | 13 ++-- .../frc/robot/vision/VisionSubsystem.java | 60 +++++++++++++++---- 3 files changed, 65 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index 8b40cda..ece379a 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -23,11 +23,18 @@ public static final class LimelightConstants { public static final boolean SPAM_BAD_DATA = true; /** The distance within which to use Limelight data in meters. */ public static final int TRUST_TAG_DISTANCE = 12; + /** Front left Limelight (April Tags) */ public static final String FRONT_APRIL_TAG_LL = "limelight-stheno"; /** Front right Limelight (Note detection) */ public static final String NOTE_DETECTION_LL = "limelight-medusa"; // TODO : configure detection limelight /** Back Limelight (April Tags) */ public static final String BACK_APRIL_TAG_LL = "limelight-euryale"; + + // Pipelines + // TODO BOT : Test pipeline downscale trust + public static final int PIPELINE_NORMAL = 0; + public static final int PIPELINE_DOWNSCALE_10_METERS = 1; + public static final int PIPELINE_DOWNSCALE_5_METERS = 2; } } diff --git a/src/main/java/frc/robot/vision/LimelightData.java b/src/main/java/frc/robot/vision/LimelightData.java index b1409d6..d637568 100644 --- a/src/main/java/frc/robot/vision/LimelightData.java +++ b/src/main/java/frc/robot/vision/LimelightData.java @@ -4,19 +4,24 @@ import frc.robot.swerve.TunerConstants; /** - * A helper class used for storing MegaTag and MegaTag2 data from a Limelight + *

A helper class used for storing MegaTag and MegaTag2 data from a Limelight * to avoid unnecessary calls to the NetworkTables API. + *

MT rotation should not be combined with MT2 pose, + * because their timestamps may differ. */ public class LimelightData { - public LimelightHelpers.PoseEstimate MegaTag; - public LimelightHelpers.PoseEstimate MegaTag2; + public final String name; + public final LimelightHelpers.PoseEstimate MegaTag; + public final LimelightHelpers.PoseEstimate MegaTag2; /** * Creates a new LimelightData object. + * @param name - The name of this Limelight. * @param MegaTag data. * @param MegaTag2 data. */ - public LimelightData(LimelightHelpers.PoseEstimate MegaTag, LimelightHelpers.PoseEstimate MegaTag2) { + public LimelightData(String name, LimelightHelpers.PoseEstimate MegaTag, LimelightHelpers.PoseEstimate MegaTag2) { + this.name = name; this.MegaTag = MegaTag; this.MegaTag2 = MegaTag2; } diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 06ebc35..a99a6d7 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -48,6 +48,7 @@ private LimelightData[] getLimelightData() { LimelightHelpers.PoseEstimate frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + LimelightData[] limelightDatas = new LimelightData[0]; double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; // If our angular velocity is greater than 360 degrees per second or there is no data, ignore vision updates @@ -56,34 +57,73 @@ private LimelightData[] getLimelightData() { && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) || (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { - return new LimelightData[0]; + return limelightDatas; } // More tags = more accurate if (backLLDataMT2 == null || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || frontLLDataMT2.tagCount > backLLDataMT2.tagCount) { - return new LimelightData[]{ new LimelightData(frontLLDataMT, frontLLDataMT2) }; + limelightDatas = new LimelightData[]{ + new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2) + }; + return optimizeLimelight(limelightDatas); } else if (frontLLDataMT2 == null || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || backLLDataMT2.tagCount > frontLLDataMT2.tagCount) { - return new LimelightData[]{ new LimelightData(backLLDataMT, backLLDataMT2) }; + limelightDatas = new LimelightData[]{ + new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) + }; + return optimizeLimelight(limelightDatas); } // If it's way closer, then trust only that one. 80% is a heuteristic if (frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.8) { - return new LimelightData[]{ new LimelightData(frontLLDataMT, frontLLDataMT2) }; + limelightDatas = new LimelightData[]{ + new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2) + }; + return optimizeLimelight(limelightDatas); } else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { - return new LimelightData[]{ new LimelightData(backLLDataMT, backLLDataMT2) }; + limelightDatas = new LimelightData[]{ + new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) + }; + return optimizeLimelight(limelightDatas); } // Both have the same amount of tags, both are almost equadistant from tags - return new LimelightData[]{ - new LimelightData(frontLLDataMT, frontLLDataMT2), - new LimelightData(backLLDataMT, backLLDataMT2) + limelightDatas = new LimelightData[]{ + new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2), + new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) }; + return optimizeLimelight(limelightDatas); + } + + /** + * A helper method used to optimize Limelight FPS. + * @param limelightDatas - The data for the Limelights to optimize. + * @return The input data for chaining. + */ + private LimelightData[] optimizeLimelight(LimelightData[] limelightDatas) { + // TODO : Smart cropping ? + // TODO : Tag filtering ? (similar to smart cropping & pipeline switching) + + for (LimelightData limelightData : limelightDatas) { + // TODO BOT : Test this + // Pipeline switching when closer to tags + if (limelightData.MegaTag2.avgTagDist < 5) { + LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_5_METERS); + } + else if (limelightData.MegaTag2.avgTagDist < 10) { + LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_10_METERS); + } + else { + LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); + } + } + + return limelightDatas; } /** @@ -146,6 +186,4 @@ public void periodic() { } } -// TODO : Smart cropping ? -// TODO : Pipeline switching ? (downscaling when closer to tags, cancelling when losing a tag) -// TODO : Tag filtering ? (similar to smart cropping & pipeline switching) \ No newline at end of file +// TODO : time methods in this file and try to improve efficiency ? \ No newline at end of file From b8b161bb42fdce6338a4ea727714927c18c91759 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Wed, 24 Jul 2024 20:35:48 -0700 Subject: [PATCH 11/23] Updated LL algorithms --- .../java/frc/robot/constants/Positions.java | 1 + .../java/frc/robot/vision/LimelightData.java | 11 +++- .../frc/robot/vision/VisionSubsystem.java | 61 +++++++++---------- 3 files changed, 39 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/constants/Positions.java b/src/main/java/frc/robot/constants/Positions.java index 0662e86..b45f512 100644 --- a/src/main/java/frc/robot/constants/Positions.java +++ b/src/main/java/frc/robot/constants/Positions.java @@ -70,6 +70,7 @@ public static Pose2d getStartingPose(PositionInitialization position) { } if (alliance.isEmpty() || position == PositionInitialization.LIMELIGHT) { + System.out.println("getStartingPose() | Using LIMELIGHT data"); return VisionSubsystem.getInstance().getEstimatedPose(); } diff --git a/src/main/java/frc/robot/vision/LimelightData.java b/src/main/java/frc/robot/vision/LimelightData.java index d637568..ca38cec 100644 --- a/src/main/java/frc/robot/vision/LimelightData.java +++ b/src/main/java/frc/robot/vision/LimelightData.java @@ -1,5 +1,6 @@ package frc.robot.vision; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import frc.robot.swerve.TunerConstants; @@ -27,13 +28,17 @@ public LimelightData(String name, LimelightHelpers.PoseEstimate MegaTag, Limelig } /** - * Checks if the average tag distance and bot's rotational velocity + * Checks if the average tag distance and bot's rotational and translational velocities * are reasonable for trusting rotation data. * @return Whether rotation data can be trusted. + * @apiNote Dist <= 3 meters ; Angular <= 160 deg/s ; Translational <= 2 */ public boolean canTrustRotation() { - double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; + ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); + double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); // 3 Meters - return this.MegaTag2.avgTagDist < 3 && Units.radiansToDegrees(angularVelocityRadians) <= 180; + return this.MegaTag2.avgTagDist <= 3 + && Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond) <= 160 + && velocity <= 2; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index a99a6d7..1bb74b1 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.PhysicalConstants.LimelightConstants; @@ -48,56 +49,48 @@ private LimelightData[] getLimelightData() { LimelightHelpers.PoseEstimate frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); - LimelightData[] limelightDatas = new LimelightData[0]; + + LimelightData[] limelightDatas = new LimelightData[]{ + new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2), + new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) + }; + optimizeLimelights(limelightDatas); - double angularVelocityRadians = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond; - // If our angular velocity is greater than 360 degrees per second or there is no data, ignore vision updates - if (Math.abs(Units.radiansToDegrees(angularVelocityRadians)) > 360 + ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); + double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); + // If our angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, or there is no data, ignore vision updates + if (Math.abs(Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond)) > 270 + || velocity <= 2 // m/s || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) || (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { - return limelightDatas; + System.out.println(true); // TODO BOT : Left off here for testing back limelight position data + return new LimelightData[0]; } // More tags = more accurate if (backLLDataMT2 == null || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || frontLLDataMT2.tagCount > backLLDataMT2.tagCount) { - limelightDatas = new LimelightData[]{ - new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2) - }; - return optimizeLimelight(limelightDatas); + return new LimelightData[]{ limelightDatas[0] }; } else if (frontLLDataMT2 == null || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || backLLDataMT2.tagCount > frontLLDataMT2.tagCount) { - limelightDatas = new LimelightData[]{ - new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) - }; - return optimizeLimelight(limelightDatas); + return new LimelightData[]{ limelightDatas[1] }; } // If it's way closer, then trust only that one. 80% is a heuteristic if (frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.8) { - limelightDatas = new LimelightData[]{ - new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2) - }; - return optimizeLimelight(limelightDatas); + return new LimelightData[]{ limelightDatas[0] }; } else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { - limelightDatas = new LimelightData[]{ - new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) - }; - return optimizeLimelight(limelightDatas); + return new LimelightData[]{ limelightDatas[1] }; } // Both have the same amount of tags, both are almost equadistant from tags - limelightDatas = new LimelightData[]{ - new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2), - new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) - }; - return optimizeLimelight(limelightDatas); + return limelightDatas; } /** @@ -105,11 +98,18 @@ else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { * @param limelightDatas - The data for the Limelights to optimize. * @return The input data for chaining. */ - private LimelightData[] optimizeLimelight(LimelightData[] limelightDatas) { + private void optimizeLimelights(LimelightData[] limelightDatas) { + if (true) return; // While testing LL position data only // TODO : Smart cropping ? // TODO : Tag filtering ? (similar to smart cropping & pipeline switching) for (LimelightData limelightData : limelightDatas) { + // Avoid unnecessary optimization for a LL with no tags + if (limelightData.MegaTag2.tagCount == 0) { + LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); + continue; + } + // TODO BOT : Test this // Pipeline switching when closer to tags if (limelightData.MegaTag2.avgTagDist < 5) { @@ -122,8 +122,6 @@ else if (limelightData.MegaTag2.avgTagDist < 10) { LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); } } - - return limelightDatas; } /** @@ -134,7 +132,8 @@ public Pose2d getEstimatedPose() { LimelightData[] limelightDatas = getLimelightData(); if (limelightDatas.length == 0) { - return null; + System.err.println("getEstimatedPose() | NO LIMELIGHT DATA, DEFAULTING TO EMTPY POSE2D"); + return new Pose2d(); } else if (limelightDatas.length == 1) { return new Pose2d( @@ -169,7 +168,7 @@ public void periodic() { for (LimelightData data : limelightDatas) { if (data.canTrustRotation()) { // Only trust rotational data - TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 0.7)); + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 0.5)); TunerConstants.DriveTrain.addVisionMeasurement( data.MegaTag.pose, data.MegaTag.timestampSeconds From 28c1c8eb98827b1be039ec795425b9dba141785a Mon Sep 17 00:00:00 2001 From: Anonyma Date: Thu, 25 Jul 2024 01:40:04 -0700 Subject: [PATCH 12/23] Possible tag filtering & likely incorrect smart cropping --- .../robot/constants/PhysicalConstants.java | 3 + .../frc/robot/vision/VisionSubsystem.java | 77 ++++++++++++++++++- 2 files changed, 76 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index ece379a..354881c 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -36,5 +36,8 @@ public static final class LimelightConstants { public static final int PIPELINE_NORMAL = 0; public static final int PIPELINE_DOWNSCALE_10_METERS = 1; public static final int PIPELINE_DOWNSCALE_5_METERS = 2; + + // All valid tag IDs + public static final int[] ALL_TAG_IDS = new int[]{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 }; } } diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 1bb74b1..c3d0f93 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -4,10 +4,16 @@ package frc.robot.vision; +import java.util.Arrays; +import java.util.HashSet; +import java.util.Set; + import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.PhysicalConstants.LimelightConstants; import frc.robot.swerve.TunerConstants; @@ -99,18 +105,81 @@ else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { * @return The input data for chaining. */ private void optimizeLimelights(LimelightData[] limelightDatas) { - if (true) return; // While testing LL position data only - // TODO : Smart cropping ? - // TODO : Tag filtering ? (similar to smart cropping & pipeline switching) + // if (true) return; // While testing LL position data only for (LimelightData limelightData : limelightDatas) { + // Avoid unnecessary optimization for a LL with no tags + // Reset any optimization that might have been done previously if (limelightData.MegaTag2.tagCount == 0) { LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); + LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, LimelightConstants.ALL_TAG_IDS); + LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); continue; } - // TODO BOT : Test this + // TODO BOT : Study tag filtering ? + Set nearbyTagsSet = new HashSet(); + for (LimelightHelpers.RawFiducial fiducial : limelightData.MegaTag2.rawFiducials) { + switch (fiducial.id) { + case 1: + case 2: + nearbyTagsSet.addAll(Arrays.asList(1, 2, 3, 4)); + break; + case 3: + case 4: + nearbyTagsSet.addAll(Arrays.asList(1, 2, 3, 4, 5)); + break; + case 5: + nearbyTagsSet.addAll(Arrays.asList(5, 4, 3)); + case 6: + nearbyTagsSet.addAll(Arrays.asList(6, 7, 8)); + break; + case 7: + case 8: + nearbyTagsSet.addAll(Arrays.asList(6, 7, 8, 9, 10)); + break; + case 9: + case 10: + nearbyTagsSet.addAll(Arrays.asList(7, 8, 9, 10)); + break; + default: // 11, 12, 13, 14, 15, and 16 have no relevant tags near them. + nearbyTagsSet.add(fiducial.id); + break; + } + } + int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); + LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); + + // TODO BOT : Study smart cropping ? + // This algorithm is likely incorrect + NetworkTable table = NetworkTableInstance.getDefault().getTable(limelightData.name); + /** Horizontal Offset From Principal Pixel To Target */ + double txnc = table.getEntry("txnc").getDouble(0); + /** Vertical Offset From Principal Pixel To Target */ + double tync = table.getEntry("txnc").getDouble(0); + /** Horizontal sidelength of the rough bounding box (0 - 320 pixels) */ + double thor = table.getEntry("thor").getDouble(0); + /** Vertical sidelength of the rough bounding box (0 - 320 pixels) */ + double tvert = table.getEntry("tvert").getDouble(0); + + // See : https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#camera-controls + if (thor == 0 || tvert == 0) { + LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); + } + else { + int halfHorAdjusted = (int) (thor <= 278 ? thor * 1.15 : 320) / 2; + int halfVertAdjusted = (int) (tvert <= 278 ? tvert * 1.15 : 320) / 2; + LimelightHelpers.setCropWindow( + limelightData.name, + (long) (tync - halfVertAdjusted) / 320, + (long) (tync + halfVertAdjusted) / 320, + (long) (txnc - halfHorAdjusted) / 320, + (long) (txnc + halfHorAdjusted) / 320 + ); + } + + // TODO BOT : Test downscaled pipeline switching ? // Pipeline switching when closer to tags if (limelightData.MegaTag2.avgTagDist < 5) { LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_5_METERS); From 7104c6dae4e808d7c14070c19662b0a907eafd5f Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Thu, 25 Jul 2024 16:56:05 -0700 Subject: [PATCH 13/23] Updated tag switching, downscaling --- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../robot/constants/PhysicalConstants.java | 6 ----- .../frc/robot/vision/VisionSubsystem.java | 25 ++++++++++--------- 3 files changed, 15 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9352aa6..4cba417 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -139,11 +139,11 @@ private void configureDrivetrain() { final Map povSpeeds = Map.ofEntries( Map.entry( 0, new Double[]{ 0.25, 0.0}), Map.entry( 45, new Double[]{ 0.25, -0.25}), - Map.entry( 90, new Double[]{ 0.0, -0.25}), + Map.entry( 90, new Double[]{ 0.0, -0.25}), Map.entry(135, new Double[]{-0.25, -0.25}), Map.entry(180, new Double[]{-0.25, 0.0}), Map.entry(225, new Double[]{-0.25, 0.25}), - Map.entry(270, new Double[]{ 0.0, 0.25}), + Map.entry(270, new Double[]{ 0.0, 0.25}), Map.entry(315, new Double[]{ 0.25, 0.25}) ); diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index 354881c..a640f47 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -31,12 +31,6 @@ public static final class LimelightConstants { /** Back Limelight (April Tags) */ public static final String BACK_APRIL_TAG_LL = "limelight-euryale"; - // Pipelines - // TODO BOT : Test pipeline downscale trust - public static final int PIPELINE_NORMAL = 0; - public static final int PIPELINE_DOWNSCALE_10_METERS = 1; - public static final int PIPELINE_DOWNSCALE_5_METERS = 2; - // All valid tag IDs public static final int[] ALL_TAG_IDS = new int[]{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 }; } diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index c3d0f93..e808f5e 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -55,7 +55,7 @@ private LimelightData[] getLimelightData() { LimelightHelpers.PoseEstimate frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); - + LimelightData[] limelightDatas = new LimelightData[]{ new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2), new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) @@ -66,12 +66,11 @@ private LimelightData[] getLimelightData() { double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); // If our angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, or there is no data, ignore vision updates if (Math.abs(Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond)) > 270 - || velocity <= 2 // m/s + || Math.abs(velocity) > 2 // m/s || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) || (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { - System.out.println(true); // TODO BOT : Left off here for testing back limelight position data return new LimelightData[0]; } @@ -111,14 +110,13 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { // Avoid unnecessary optimization for a LL with no tags // Reset any optimization that might have been done previously - if (limelightData.MegaTag2.tagCount == 0) { - LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); + if (limelightData.MegaTag2 == null || limelightData.MegaTag2.tagCount == 0) { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, LimelightConstants.ALL_TAG_IDS); LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); continue; } - // TODO BOT : Study tag filtering ? Set nearbyTagsSet = new HashSet(); for (LimelightHelpers.RawFiducial fiducial : limelightData.MegaTag2.rawFiducials) { switch (fiducial.id) { @@ -132,6 +130,7 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { break; case 5: nearbyTagsSet.addAll(Arrays.asList(5, 4, 3)); + break; case 6: nearbyTagsSet.addAll(Arrays.asList(6, 7, 8)); break; @@ -179,16 +178,18 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { ); } - // TODO BOT : Test downscaled pipeline switching ? // Pipeline switching when closer to tags - if (limelightData.MegaTag2.avgTagDist < 5) { - LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_5_METERS); + if (limelightData.MegaTag2.avgTagDist < 1.75) { + // LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_5_METERS); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 4); } - else if (limelightData.MegaTag2.avgTagDist < 10) { - LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_10_METERS); + else if (limelightData.MegaTag2.avgTagDist < 2.5) { + // LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_10_METERS); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); } else { - LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); + // LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); } } } From 1109d34e15644b16c9430a5e79e5ee19e884df83 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Mon, 29 Jul 2024 14:40:43 -0700 Subject: [PATCH 14/23] Updated constants, stub for new smart cropping --- .../robot/constants/PhysicalConstants.java | 3 +++ .../frc/robot/vision/VisionSubsystem.java | 27 ++++--------------- 2 files changed, 8 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index a640f47..e2e68d6 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -33,5 +33,8 @@ public static final class LimelightConstants { // All valid tag IDs public static final int[] ALL_TAG_IDS = new int[]{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 }; + // Field of view of LL3G, used for Smart Cropping + public static final double FOV_X = 82; + public static final double FOV_Y = 56.2; } } diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index e808f5e..5f32a64 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -12,8 +12,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.PhysicalConstants.LimelightConstants; import frc.robot.swerve.TunerConstants; @@ -150,31 +148,16 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); - // TODO BOT : Study smart cropping ? - // This algorithm is likely incorrect - NetworkTable table = NetworkTableInstance.getDefault().getTable(limelightData.name); - /** Horizontal Offset From Principal Pixel To Target */ - double txnc = table.getEntry("txnc").getDouble(0); - /** Vertical Offset From Principal Pixel To Target */ - double tync = table.getEntry("txnc").getDouble(0); - /** Horizontal sidelength of the rough bounding box (0 - 320 pixels) */ - double thor = table.getEntry("thor").getDouble(0); - /** Vertical sidelength of the rough bounding box (0 - 320 pixels) */ - double tvert = table.getEntry("tvert").getDouble(0); - - // See : https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#camera-controls - if (thor == 0 || tvert == 0) { + // TODO BOT : Fix smart cropping ? + // See : https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#basic-targeting-data + if (limelightData.MegaTag2.rawFiducials.length == 0) { LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); } else { - int halfHorAdjusted = (int) (thor <= 278 ? thor * 1.15 : 320) / 2; - int halfVertAdjusted = (int) (tvert <= 278 ? tvert * 1.15 : 320) / 2; + // Use limelightData.MegaTag2.rawFiducials[] LimelightHelpers.setCropWindow( limelightData.name, - (long) (tync - halfVertAdjusted) / 320, - (long) (tync + halfVertAdjusted) / 320, - (long) (txnc - halfHorAdjusted) / 320, - (long) (txnc + halfHorAdjusted) / 320 + 0, 0, 0, 0 ); } From ed0a143678c1e36bec6f425c03e2cff66515cbd5 Mon Sep 17 00:00:00 2001 From: Anonyma Date: Mon, 29 Jul 2024 15:55:34 -0700 Subject: [PATCH 15/23] New smart cropping to test --- .../frc/robot/vision/VisionSubsystem.java | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 5f32a64..dc25b85 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -155,23 +155,51 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { } else { // Use limelightData.MegaTag2.rawFiducials[] - LimelightHelpers.setCropWindow( - limelightData.name, - 0, 0, 0, 0 - ); + LimelightHelpers.RawFiducial txncBig = null; + LimelightHelpers.RawFiducial txncSmall = null; + LimelightHelpers.RawFiducial tyncBig = null; + LimelightHelpers.RawFiducial tyncSmall = null; + double targetSize = 0; + + for (LimelightHelpers.RawFiducial fiducial: limelightData.MegaTag2.rawFiducials) { + targetSize = Math.sqrt(fiducial.ta * LimelightConstants.FOV_X * LimelightConstants.FOV_Y) / 2; + System.out.printf("ta %f ; size %f%n", fiducial.ta, targetSize); + if (true) return; + + if (txncBig == null || fiducial.txnc + targetSize > txncBig.txnc) { + txncBig = fiducial; + } + else if (txncSmall == null || fiducial.txnc - targetSize < txncSmall.txnc) { + txncSmall = fiducial; + } + + if (tyncBig == null || fiducial.tync + targetSize > tyncBig.tync) { + tyncBig = fiducial; + } + else if (tyncSmall == null || fiducial.tync - targetSize < tyncSmall.tync) { + tyncSmall = fiducial; + } + } + + System.out.printf("txnc small %f ; txnc big %f, tync small %f, tync big %f%n", + txncSmall.txnc, txncBig.txnc, tyncSmall.tync, tyncBig.tync); + System.out.printf("txnc small %d ; txnc big %d, tync small %d, tync big %d%n", + txncSmall.id, txncBig.id, tyncSmall.id, tyncBig.id); + + // LimelightHelpers.setCropWindow( + // limelightData.name, + + // ); } // Pipeline switching when closer to tags if (limelightData.MegaTag2.avgTagDist < 1.75) { - // LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_5_METERS); LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 4); } else if (limelightData.MegaTag2.avgTagDist < 2.5) { - // LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_DOWNSCALE_10_METERS); LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); } else { - // LimelightHelpers.setPipelineIndex(limelightData.name, LimelightConstants.PIPELINE_NORMAL); LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); } } From 76a8ef6f0a6a91938b7b6d89ded0a697dee2a711 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Mon, 29 Jul 2024 19:16:39 -0700 Subject: [PATCH 16/23] Smart cropping --- .../robot/constants/PhysicalConstants.java | 1 + .../frc/robot/vision/VisionSubsystem.java | 75 ++++++++++++------- 2 files changed, 50 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index e2e68d6..765d237 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -36,5 +36,6 @@ public static final class LimelightConstants { // Field of view of LL3G, used for Smart Cropping public static final double FOV_X = 82; public static final double FOV_Y = 56.2; + public static final double FOV_AREA = FOV_X * FOV_Y; } } diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index dc25b85..e497eda 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -7,6 +7,7 @@ import java.util.Arrays; import java.util.HashSet; import java.util.Set; +import java.util.function.Function; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -35,6 +36,27 @@ public static VisionSubsystem getInstance() { return instance; } + /** + * Tags a tag ID and returns whether to expect one on the left, right, or neither. + * Left : -1 ; Right : +1 ; Neither : 0 + */ + private Function expandDirection = (Integer id) -> { + switch (id) { + case 1: + case 3: + case 7: + case 9: + return -1; + case 2: + case 4: + case 8: + case 10: + return 1; + default: + return 0; + } + }; + /** Creates a new ExampleSubsystem. */ private VisionSubsystem() { super("VisionSubsystem"); @@ -102,19 +124,17 @@ else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { * @return The input data for chaining. */ private void optimizeLimelights(LimelightData[] limelightDatas) { - // if (true) return; // While testing LL position data only - for (LimelightData limelightData : limelightDatas) { - // Avoid unnecessary optimization for a LL with no tags // Reset any optimization that might have been done previously if (limelightData.MegaTag2 == null || limelightData.MegaTag2.tagCount == 0) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, LimelightConstants.ALL_TAG_IDS); LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); continue; } + // Tag filtering Set nearbyTagsSet = new HashSet(); for (LimelightHelpers.RawFiducial fiducial : limelightData.MegaTag2.rawFiducials) { switch (fiducial.id) { @@ -148,7 +168,7 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); - // TODO BOT : Fix smart cropping ? + // Smart cropping follows AprilTags // See : https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#basic-targeting-data if (limelightData.MegaTag2.rawFiducials.length == 0) { LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); @@ -160,47 +180,50 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { LimelightHelpers.RawFiducial tyncBig = null; LimelightHelpers.RawFiducial tyncSmall = null; double targetSize = 0; - + for (LimelightHelpers.RawFiducial fiducial: limelightData.MegaTag2.rawFiducials) { - targetSize = Math.sqrt(fiducial.ta * LimelightConstants.FOV_X * LimelightConstants.FOV_Y) / 2; - System.out.printf("ta %f ; size %f%n", fiducial.ta, targetSize); - if (true) return; + targetSize = Math.sqrt(fiducial.ta * LimelightConstants.FOV_AREA) / 2; if (txncBig == null || fiducial.txnc + targetSize > txncBig.txnc) { txncBig = fiducial; } - else if (txncSmall == null || fiducial.txnc - targetSize < txncSmall.txnc) { + if (txncSmall == null || fiducial.txnc - targetSize < txncSmall.txnc) { txncSmall = fiducial; } - + if (tyncBig == null || fiducial.tync + targetSize > tyncBig.tync) { tyncBig = fiducial; } - else if (tyncSmall == null || fiducial.tync - targetSize < tyncSmall.tync) { + if (tyncSmall == null || fiducial.tync - targetSize < tyncSmall.tync) { tyncSmall = fiducial; } } - - System.out.printf("txnc small %f ; txnc big %f, tync small %f, tync big %f%n", - txncSmall.txnc, txncBig.txnc, tyncSmall.tync, tyncBig.tync); - System.out.printf("txnc small %d ; txnc big %d, tync small %d, tync big %d%n", - txncSmall.id, txncBig.id, tyncSmall.id, tyncBig.id); - - // LimelightHelpers.setCropWindow( - // limelightData.name, - - // ); + + double xSmall = (txncSmall.txnc - Math.sqrt(txncSmall.ta * LimelightConstants.FOV_AREA) * (1.75 * Math.log(txncSmall.distToCamera + 1))) + / (LimelightConstants.FOV_X / 2); + double xBig = (txncBig.txnc + Math.sqrt(txncBig.ta * LimelightConstants.FOV_AREA) * (1.75 * Math.log(txncBig.distToCamera + 1))) + / (LimelightConstants.FOV_X / 2); + + LimelightHelpers.setCropWindow( + limelightData.name, + this.expandDirection.apply(txncSmall.id) < 0 ? xSmall - 1.5 * Math.abs(xBig - xSmall) : xSmall, + this.expandDirection.apply(txncBig.id) > 0 ? xBig + 1.5 * Math.abs(xBig - xSmall) : xBig, + (tyncSmall.tync - Math.sqrt(tyncSmall.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(tyncBig.distToCamera + 1))) + / (LimelightConstants.FOV_Y / 2), + (tyncBig.tync + Math.sqrt(tyncBig.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(tyncSmall.distToCamera + 1))) + / (LimelightConstants.FOV_Y / 2) + ); } - // Pipeline switching when closer to tags + // Downscaling closer to tags if (limelightData.MegaTag2.avgTagDist < 1.75) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 4); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); } else if (limelightData.MegaTag2.avgTagDist < 2.5) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); } else { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); } } } From 0de67b9d93bfb50125c8a843210485c6cf3865b1 Mon Sep 17 00:00:00 2001 From: Anonyma Date: Tue, 30 Jul 2024 01:17:03 -0700 Subject: [PATCH 17/23] Avoid redundant position update / optimization --- .../java/frc/robot/vision/LimelightData.java | 3 + .../frc/robot/vision/VisionSubsystem.java | 209 +++++++++++------- 2 files changed, 137 insertions(+), 75 deletions(-) diff --git a/src/main/java/frc/robot/vision/LimelightData.java b/src/main/java/frc/robot/vision/LimelightData.java index ca38cec..0b7858f 100644 --- a/src/main/java/frc/robot/vision/LimelightData.java +++ b/src/main/java/frc/robot/vision/LimelightData.java @@ -14,6 +14,8 @@ public class LimelightData { public final String name; public final LimelightHelpers.PoseEstimate MegaTag; public final LimelightHelpers.PoseEstimate MegaTag2; + /** Flag set after optimization to avoid re-optimizing data twice in a row on low FPS. */ + public boolean optimized; /** * Creates a new LimelightData object. @@ -25,6 +27,7 @@ public LimelightData(String name, LimelightHelpers.PoseEstimate MegaTag, Limelig this.name = name; this.MegaTag = MegaTag; this.MegaTag2 = MegaTag2; + this.optimized = false; } /** diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index e497eda..833d514 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -6,8 +6,8 @@ import java.util.Arrays; import java.util.HashSet; +import java.util.List; import java.util.Set; -import java.util.function.Function; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -36,26 +36,20 @@ public static VisionSubsystem getInstance() { return instance; } - /** - * Tags a tag ID and returns whether to expect one on the left, right, or neither. - * Left : -1 ; Right : +1 ; Neither : 0 - */ - private Function expandDirection = (Integer id) -> { - switch (id) { - case 1: - case 3: - case 7: - case 9: - return -1; - case 2: - case 4: - case 8: - case 10: - return 1; - default: - return 0; - } - }; + /** Latest Limelight data. May contain faulty data unsuitable for odometry. */ + private LimelightData[] limelightDatas = new LimelightData[2]; + /** Last heartbeat of the front LL (updated every frame) */ + private long lastHeartbeatStheno = 0; + /** Last heartbeat of the back LL (updated every frame) */ + private long lastHeartbeatEuryale = 0; + + // Lists used for tag filtering. Final to reduce wasted processing power. + private final List BLUE_SOURCE = Arrays.asList(1, 2, 3, 4); + private final List RED_SPEAKER = Arrays.asList(1, 2, 3, 4, 5); + private final List RED_AMP = Arrays.asList(5, 4, 3); + private final List BLUE_AMP = Arrays.asList(6, 7, 8); + private final List BLUE_SPEAKER = Arrays.asList(6, 7, 8, 9, 10); + private final List RED_SOURCE = Arrays.asList(7, 8, 9, 10); /** Creates a new ExampleSubsystem. */ private VisionSubsystem() { @@ -63,30 +57,42 @@ private VisionSubsystem() { } /** - * Gets the most trustworthy data from each Limelight. + * Gets the most trustworthy data from each Limelight. Also updates {@link VisionSubsystem#limelightDatas} and heartbeats. * @return LimelightData for each trustworthy Limelight data. + * @apiNote Will theoretically stop updating data when the heartbeat has reset. + * However, this happens at 2e9 frames, which would take 96 days at a consistent 240 fps . */ private LimelightData[] getLimelightData() { double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); - LimelightHelpers.PoseEstimate frontLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate frontLLDataMT2 = null; + LimelightHelpers.PoseEstimate backLLDataMT2 = null; + long heartbeatStheno = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.FRONT_APRIL_TAG_LL, "hb").getInteger(-1); + long heartbeatEuryale = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.BACK_APRIL_TAG_LL, "hb").getInteger(-1); - LimelightData[] limelightDatas = new LimelightData[]{ - new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2), - new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2) - }; - optimizeLimelights(limelightDatas); + if (heartbeatStheno == -1 || this.lastHeartbeatStheno < heartbeatStheno) { + frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate frontLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL); + this.limelightDatas[0] = new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2); + this.lastHeartbeatStheno = heartbeatStheno == -1 ? this.lastHeartbeatStheno : heartbeatStheno; + } + + if (heartbeatEuryale == -1 || this.lastHeartbeatEuryale < heartbeatEuryale) { + backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); + this.limelightDatas[1] = new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2); + this.lastHeartbeatEuryale = heartbeatEuryale == -1 ? this.lastHeartbeatEuryale : heartbeatEuryale; + } ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); - // If our angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, or there is no data, ignore vision updates + // If our angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, + // the data is outdated (no new heartbeat), or there is no data, ignore vision updates. if (Math.abs(Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond)) > 270 || Math.abs(velocity) > 2 // m/s + || (this.lastHeartbeatEuryale != heartbeatEuryale && this.lastHeartbeatStheno != heartbeatStheno) || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) || (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE @@ -94,37 +100,42 @@ private LimelightData[] getLimelightData() { return new LimelightData[0]; } - // More tags = more accurate - if (backLLDataMT2 == null + // More tags = more accurate. Also eliminate the other if it happens to be invalid. + if (this.lastHeartbeatEuryale != heartbeatEuryale + || backLLDataMT2 == null || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || frontLLDataMT2.tagCount > backLLDataMT2.tagCount) { - return new LimelightData[]{ limelightDatas[0] }; + return new LimelightData[]{ this.limelightDatas[0] }; } - else if (frontLLDataMT2 == null + else if (this.lastHeartbeatStheno != heartbeatStheno + || frontLLDataMT2 == null || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || backLLDataMT2.tagCount > frontLLDataMT2.tagCount) { - return new LimelightData[]{ limelightDatas[1] }; + return new LimelightData[]{ this.limelightDatas[1] }; } // If it's way closer, then trust only that one. 80% is a heuteristic if (frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.8) { - return new LimelightData[]{ limelightDatas[0] }; + return new LimelightData[]{ this.limelightDatas[0] }; } else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { - return new LimelightData[]{ limelightDatas[1] }; + return new LimelightData[]{ this.limelightDatas[1] }; } // Both have the same amount of tags, both are almost equadistant from tags - return limelightDatas; + return this.limelightDatas; } /** * A helper method used to optimize Limelight FPS. - * @param limelightDatas - The data for the Limelights to optimize. - * @return The input data for chaining. */ - private void optimizeLimelights(LimelightData[] limelightDatas) { - for (LimelightData limelightData : limelightDatas) { + private void optimizeLimelights() { + byte index = 0; // Used only for setting the optimized flag, so that this can be a for-each loop. + for (LimelightData limelightData : this.limelightDatas) { + if (!limelightData.optimized) { + this.limelightDatas[index++].optimized = true; + } + // Avoid unnecessary optimization for a LL with no tags // Reset any optimization that might have been done previously if (limelightData.MegaTag2 == null || limelightData.MegaTag2.tagCount == 0) { @@ -140,33 +151,37 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { switch (fiducial.id) { case 1: case 2: - nearbyTagsSet.addAll(Arrays.asList(1, 2, 3, 4)); - break; + nearbyTagsSet.addAll(this.BLUE_SOURCE); case 3: case 4: - nearbyTagsSet.addAll(Arrays.asList(1, 2, 3, 4, 5)); - break; + nearbyTagsSet.addAll(this.RED_SPEAKER); case 5: - nearbyTagsSet.addAll(Arrays.asList(5, 4, 3)); - break; + nearbyTagsSet.addAll(this.RED_AMP); case 6: - nearbyTagsSet.addAll(Arrays.asList(6, 7, 8)); - break; + nearbyTagsSet.addAll(this.BLUE_AMP); case 7: case 8: - nearbyTagsSet.addAll(Arrays.asList(6, 7, 8, 9, 10)); - break; + nearbyTagsSet.addAll(this.BLUE_SPEAKER); case 9: case 10: - nearbyTagsSet.addAll(Arrays.asList(7, 8, 9, 10)); - break; + nearbyTagsSet.addAll(this.RED_SOURCE); default: // 11, 12, 13, 14, 15, and 16 have no relevant tags near them. nearbyTagsSet.add(fiducial.id); - break; } } int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); + + // Downscaling closer to tags + if (limelightData.MegaTag2.avgTagDist < 1.75) { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); + } + else if (limelightData.MegaTag2.avgTagDist < 2.5) { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); + } + else { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); + } // Smart cropping follows AprilTags // See : https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#basic-targeting-data @@ -174,13 +189,14 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); } else { - // Use limelightData.MegaTag2.rawFiducials[] LimelightHelpers.RawFiducial txncBig = null; LimelightHelpers.RawFiducial txncSmall = null; LimelightHelpers.RawFiducial tyncBig = null; LimelightHelpers.RawFiducial tyncSmall = null; double targetSize = 0; + // Finds the txnc and tync that are furthest from the crosshair + // (for largest bounding box that will include all targets on screen) for (LimelightHelpers.RawFiducial fiducial: limelightData.MegaTag2.rawFiducials) { targetSize = Math.sqrt(fiducial.ta * LimelightConstants.FOV_AREA) / 2; @@ -199,34 +215,66 @@ private void optimizeLimelights(LimelightData[] limelightDatas) { } } + // The formulas used below create the bounding boxes around targets and work in this way : + // + // The position of the target (x or y) that was found to be the furthest from the principal pixel for that direction + // (largest/smallest x and largest/smallest y) + // MINUS for the smallest positions (left/bottom of the box) or PLUS for the largest positions (right/top of the box) + // The length of the side of the targets — This is found in the following way : + // We know the FOV area (LimelightConstants.FOV_AREA) -> We know percentage of screen target occupies (ta) -> + // Targets are roughly squares at most angles so sqrt(target area in pixels) = side lengths + // Which is MULTIPLIED by a function that scales with distance (further away needs larger box due + // to bot movements having more impact on target position from the camera's POV) in the following way : + // 1.75 (heuteristic, this determines general box size) * ln(distance to target + 1) + // The +1 is added to the natural log to avoid a negative value for distances of less than 1 meter, + // even if those are very rare. Natural log is probably not the best function for this, but it works well enough. + // + // Together this comes out as (be careful to follow order of operations) : + // Target Position +/- Target Length * (1.75 * ln(Distance + 1)) + // + // In the end this is DIVIDED by HALF of the rough width or height of the FOV, + // because Limelights expect cropping to be [-1.0, 1.0]. + double xSmall = (txncSmall.txnc - Math.sqrt(txncSmall.ta * LimelightConstants.FOV_AREA) * (1.75 * Math.log(txncSmall.distToCamera + 1))) - / (LimelightConstants.FOV_X / 2); + / (LimelightConstants.FOV_X / 2); double xBig = (txncBig.txnc + Math.sqrt(txncBig.ta * LimelightConstants.FOV_AREA) * (1.75 * Math.log(txncBig.distToCamera + 1))) - / (LimelightConstants.FOV_X / 2); + / (LimelightConstants.FOV_X / 2); LimelightHelpers.setCropWindow( limelightData.name, - this.expandDirection.apply(txncSmall.id) < 0 ? xSmall - 1.5 * Math.abs(xBig - xSmall) : xSmall, - this.expandDirection.apply(txncBig.id) > 0 ? xBig + 1.5 * Math.abs(xBig - xSmall) : xBig, + getNearbyTagDirection(txncSmall.id) < 0 ? xSmall - 1.5 * Math.abs(xBig - xSmall) : xSmall, + getNearbyTagDirection(txncBig.id) > 0 ? xBig + 1.5 * Math.abs(xBig - xSmall) : xBig, (tyncSmall.tync - Math.sqrt(tyncSmall.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(tyncBig.distToCamera + 1))) / (LimelightConstants.FOV_Y / 2), (tyncBig.tync + Math.sqrt(tyncBig.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(tyncSmall.distToCamera + 1))) / (LimelightConstants.FOV_Y / 2) ); } - - // Downscaling closer to tags - if (limelightData.MegaTag2.avgTagDist < 1.75) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); - } - else if (limelightData.MegaTag2.avgTagDist < 2.5) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); - } - else { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); - } } } + + /** + * This is a helper for {@link VisionSubsystem#optimizeLimelights(LimelightData[])} smart cropping. + * @param id - The target ID to consider. + * @return Whether to expect another tag on the left, right, or neither. + * @apiNote Left : -1 ; Right : +1 ; Neither : 0. + */ + private int getNearbyTagDirection(int id) { + switch (id) { + case 1: + case 3: + case 7: + case 9: + return -1; + case 2: + case 4: + case 8: + case 10: + return 1; + default: + return 0; + } + }; /** * Gets the most trustworthy data from each Limelight and returns a {@link Pose2d} object. @@ -267,8 +315,13 @@ else if (limelightDatas.length == 1) { // This method will be called once per scheduler run @Override public void periodic() { + // TODO BOT : Timing + long startTime = System.currentTimeMillis(); + LimelightData[] limelightDatas = getLimelightData(); + System.out.printf("Get data : %d ms%n", System.currentTimeMillis() - startTime); + for (LimelightData data : limelightDatas) { if (data.canTrustRotation()) { // Only trust rotational data @@ -286,7 +339,13 @@ public void periodic() { data.MegaTag2.timestampSeconds ); } + + System.out.printf("Update position : %d ms%n", System.currentTimeMillis() - startTime); + + long optiStartTime = System.currentTimeMillis(); + optimizeLimelights(); + long finalTime = System.currentTimeMillis(); + System.out.printf("Optimize LLs : %d ms%n", finalTime - optiStartTime); + System.out.printf("Final : %d ms%n", finalTime - startTime); } } - -// TODO : time methods in this file and try to improve efficiency ? \ No newline at end of file From e2afe3acef6a7698b776c0491cefa6e072ed4e09 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Tue, 30 Jul 2024 03:26:54 -0700 Subject: [PATCH 18/23] finished LL position & optimization !! --- .../frc/robot/vision/VisionSubsystem.java | 63 +++++++++---------- 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 833d514..7e78cc7 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -95,30 +95,35 @@ private LimelightData[] getLimelightData() { || (this.lastHeartbeatEuryale != heartbeatEuryale && this.lastHeartbeatStheno != heartbeatStheno) || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) - || (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE - && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { + || (frontLLDataMT2 != null && frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + && backLLDataMT2 != null && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { return new LimelightData[0]; } + if (frontLLDataMT2 == null) { + frontLLDataMT2 = this.limelightDatas[0].MegaTag2; + } + if (backLLDataMT2 == null) { + backLLDataMT2 = this.limelightDatas[1].MegaTag2; + } + // More tags = more accurate. Also eliminate the other if it happens to be invalid. - if (this.lastHeartbeatEuryale != heartbeatEuryale - || backLLDataMT2 == null - || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE - || frontLLDataMT2.tagCount > backLLDataMT2.tagCount) { + if (this.lastHeartbeatStheno == heartbeatStheno + && (backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + || frontLLDataMT2.tagCount > backLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if (this.lastHeartbeatStheno != heartbeatStheno - || frontLLDataMT2 == null - || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE - || backLLDataMT2.tagCount > frontLLDataMT2.tagCount) { + else if (this.lastHeartbeatEuryale == heartbeatEuryale + && (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + || backLLDataMT2.tagCount > frontLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[1] }; } - // If it's way closer, then trust only that one. 80% is a heuteristic - if (frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.8) { + // If it's way closer, then trust only that one. 90% is a heuteristic + if (this.lastHeartbeatStheno == heartbeatStheno && frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.9) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if (backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.8) { + else if (this.lastHeartbeatEuryale == heartbeatEuryale && backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.9) { return new LimelightData[]{ this.limelightDatas[1] }; } @@ -135,6 +140,9 @@ private void optimizeLimelights() { if (!limelightData.optimized) { this.limelightDatas[index++].optimized = true; } + else { + return; + } // Avoid unnecessary optimization for a LL with no tags // Reset any optimization that might have been done previously @@ -225,19 +233,19 @@ else if (limelightData.MegaTag2.avgTagDist < 2.5) { // Targets are roughly squares at most angles so sqrt(target area in pixels) = side lengths // Which is MULTIPLIED by a function that scales with distance (further away needs larger box due // to bot movements having more impact on target position from the camera's POV) in the following way : - // 1.75 (heuteristic, this determines general box size) * ln(distance to target + 1) + // 2 (heuteristic, this determines general box size) * ln(distance to target + 1) // The +1 is added to the natural log to avoid a negative value for distances of less than 1 meter, // even if those are very rare. Natural log is probably not the best function for this, but it works well enough. // // Together this comes out as (be careful to follow order of operations) : - // Target Position +/- Target Length * (1.75 * ln(Distance + 1)) + // Target Position +/- Target Length * (2 * ln(Distance + 1)) // // In the end this is DIVIDED by HALF of the rough width or height of the FOV, // because Limelights expect cropping to be [-1.0, 1.0]. - double xSmall = (txncSmall.txnc - Math.sqrt(txncSmall.ta * LimelightConstants.FOV_AREA) * (1.75 * Math.log(txncSmall.distToCamera + 1))) + double xSmall = (txncSmall.txnc - Math.sqrt(txncSmall.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(txncSmall.distToCamera + 1))) / (LimelightConstants.FOV_X / 2); - double xBig = (txncBig.txnc + Math.sqrt(txncBig.ta * LimelightConstants.FOV_AREA) * (1.75 * Math.log(txncBig.distToCamera + 1))) + double xBig = (txncBig.txnc + Math.sqrt(txncBig.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(txncBig.distToCamera + 1))) / (LimelightConstants.FOV_X / 2); LimelightHelpers.setCropWindow( @@ -301,10 +309,9 @@ else if (limelightDatas.length == 1) { limelightDatas[0].MegaTag2.pose.getTranslation().plus(limelightDatas[1].MegaTag2.pose.getTranslation()).div(2), limelightDatas[0].canTrustRotation() ? // First rotation / 2 + Second rotation / 2 - /* - * This is done to avoid problems due to Rotation2d being [0, 360) - * Ex : 180+180=0 followed by 0/2=0 when it should be 180+180=360 and 360/2=180. - */ + // + // This is done to avoid problems due to Rotation2d being [0, 360) + // Ex : 180+180=0 followed by 0/2=0 when it should be 180+180=360 and 360/2=180. limelightDatas[0].MegaTag.pose.getRotation().div(2) .plus(limelightDatas[1].MegaTag.pose.getRotation().div(2)) : TunerConstants.DriveTrain.getState().Pose.getRotation() @@ -315,13 +322,10 @@ else if (limelightDatas.length == 1) { // This method will be called once per scheduler run @Override public void periodic() { - // TODO BOT : Timing - long startTime = System.currentTimeMillis(); - + // This method variably gets data in between 4-10 ms. LimelightData[] limelightDatas = getLimelightData(); - System.out.printf("Get data : %d ms%n", System.currentTimeMillis() - startTime); - + // This loop generally updates data in 6 ms, but may double or triple for no apparent reason. for (LimelightData data : limelightDatas) { if (data.canTrustRotation()) { // Only trust rotational data @@ -339,13 +343,8 @@ public void periodic() { data.MegaTag2.timestampSeconds ); } - - System.out.printf("Update position : %d ms%n", System.currentTimeMillis() - startTime); - long optiStartTime = System.currentTimeMillis(); + // This method is suprprisingly efficient, generally below 1 ms. optimizeLimelights(); - long finalTime = System.currentTimeMillis(); - System.out.printf("Optimize LLs : %d ms%n", finalTime - optiStartTime); - System.out.printf("Final : %d ms%n", finalTime - startTime); } } From 863b9779c3de340babaa08d2577b6f9b41c19eea Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Tue, 30 Jul 2024 11:03:39 -0700 Subject: [PATCH 19/23] Javadoc / general wording fixes --- .../robot/constants/PhysicalConstants.java | 24 ++-- .../java/frc/robot/vision/LimelightData.java | 5 +- .../frc/robot/vision/VisionSubsystem.java | 129 ++++++++++-------- 3 files changed, 89 insertions(+), 69 deletions(-) diff --git a/src/main/java/frc/robot/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index 765d237..3e1992a 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -6,36 +6,40 @@ /** * Constants used throughout the code specifically related to subsystems or unchangeable aspects of the bot. - * @implNote BACK of bot is shooter/battery/180 deg, use that as reference for directions. + * @implSpec BACK of the bot is 180 degrees / the battery, use that as a reference for directions. */ public final class PhysicalConstants { /** - * Constants of physical attributes of the robot. + * Constants for physical attributes of the robot. */ public static final class RobotConstants { - /** Name of the CTRE CAN bus. */ + /** Name of the CTRE CAN bus (configured on the CANivore). */ public static final String CTRE_CAN_BUS = "ctre"; } /** Constants for limelight-related data. */ public static final class LimelightConstants { - /** Spams "Bad LL 2D/3D Pose Data" when no data is coming from the NetworkTableInstance for LL. */ + /** Spams "Bad LL 2D/3D Pose Data" when no data is coming from the NetworkTableInstance for a LL. */ public static final boolean SPAM_BAD_DATA = true; - /** The distance within which to use Limelight data in meters. */ + /** The distance within which to use Limelight data in meters. This is measured from tag to camera.*/ public static final int TRUST_TAG_DISTANCE = 12; - /** Front left Limelight (April Tags) */ + /** Front left Limelight (April Tags). */ public static final String FRONT_APRIL_TAG_LL = "limelight-stheno"; - /** Front right Limelight (Note detection) */ + /** Front right Limelight (Note detection). */ public static final String NOTE_DETECTION_LL = "limelight-medusa"; // TODO : configure detection limelight - /** Back Limelight (April Tags) */ + /** Back Limelight (April Tags). */ public static final String BACK_APRIL_TAG_LL = "limelight-euryale"; - // All valid tag IDs + /** All valid tag IDs (used for tag filtering) */ public static final int[] ALL_TAG_IDS = new int[]{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 }; - // Field of view of LL3G, used for Smart Cropping + + // See https://docs.limelightvision.io/docs/docs-limelight/hardware-comparison. + /** Horizontal FOV of LL3G in degrees (used for smart cropping) */ public static final double FOV_X = 82; + /** Vertical FOV of LL3G in degrees (used for smart cropping) */ public static final double FOV_Y = 56.2; + /** FOV area of the LL3g in degrees squared (used for smart cropping) */ public static final double FOV_AREA = FOV_X * FOV_Y; } } diff --git a/src/main/java/frc/robot/vision/LimelightData.java b/src/main/java/frc/robot/vision/LimelightData.java index 0b7858f..dbaa260 100644 --- a/src/main/java/frc/robot/vision/LimelightData.java +++ b/src/main/java/frc/robot/vision/LimelightData.java @@ -7,8 +7,7 @@ /** *

A helper class used for storing MegaTag and MegaTag2 data from a Limelight * to avoid unnecessary calls to the NetworkTables API. - *

MT rotation should not be combined with MT2 pose, - * because their timestamps may differ. + *

MT rotation should not be combined with MT2 pose, because their timestamps may differ. */ public class LimelightData { public final String name; @@ -34,7 +33,7 @@ public LimelightData(String name, LimelightHelpers.PoseEstimate MegaTag, Limelig * Checks if the average tag distance and bot's rotational and translational velocities * are reasonable for trusting rotation data. * @return Whether rotation data can be trusted. - * @apiNote Dist <= 3 meters ; Angular <= 160 deg/s ; Translational <= 2 + * @apiNote Dist <= 3 meters ; Angular <= 160 deg/s ; Translational <= 2. */ public boolean canTrustRotation() { ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 7e78cc7..bbcbd64 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -39,9 +39,9 @@ public static VisionSubsystem getInstance() { /** Latest Limelight data. May contain faulty data unsuitable for odometry. */ private LimelightData[] limelightDatas = new LimelightData[2]; /** Last heartbeat of the front LL (updated every frame) */ - private long lastHeartbeatStheno = 0; + private long lastHeartbeatFrontLL = 0; /** Last heartbeat of the back LL (updated every frame) */ - private long lastHeartbeatEuryale = 0; + private long lastHeartbeatBackLL = 0; // Lists used for tag filtering. Final to reduce wasted processing power. private final List BLUE_SOURCE = Arrays.asList(1, 2, 3, 4); @@ -51,18 +51,20 @@ public static VisionSubsystem getInstance() { private final List BLUE_SPEAKER = Arrays.asList(6, 7, 8, 9, 10); private final List RED_SOURCE = Arrays.asList(7, 8, 9, 10); - /** Creates a new ExampleSubsystem. */ + /** Creates a new VisionSubsystem. */ private VisionSubsystem() { super("VisionSubsystem"); } /** * Gets the most trustworthy data from each Limelight. Also updates {@link VisionSubsystem#limelightDatas} and heartbeats. + * @param ignoreHeartbeat - This will only ignore heartbeats for old data, meaning that this method is allowed to return stale/stored data. + * It will not ignore hearbeats when querying new data. * @return LimelightData for each trustworthy Limelight data. - * @apiNote Will theoretically stop updating data when the heartbeat has reset. - * However, this happens at 2e9 frames, which would take 96 days at a consistent 240 fps . + * @apiNote Will theoretically stop updating data if the heartbeat resets. + * However, this happens at 2e9 frames, which would take consecutive 96 days at a consistent 240 fps. */ - private LimelightData[] getLimelightData() { + private LimelightData[] getFilteredLimelightData(boolean ignoreHeartbeat) { double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); @@ -72,27 +74,27 @@ private LimelightData[] getLimelightData() { long heartbeatStheno = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.FRONT_APRIL_TAG_LL, "hb").getInteger(-1); long heartbeatEuryale = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.BACK_APRIL_TAG_LL, "hb").getInteger(-1); - if (heartbeatStheno == -1 || this.lastHeartbeatStheno < heartbeatStheno) { + if (heartbeatStheno == -1 || this.lastHeartbeatFrontLL < heartbeatStheno) { frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); LimelightHelpers.PoseEstimate frontLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL); this.limelightDatas[0] = new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2); - this.lastHeartbeatStheno = heartbeatStheno == -1 ? this.lastHeartbeatStheno : heartbeatStheno; + this.lastHeartbeatFrontLL = heartbeatStheno == -1 ? this.lastHeartbeatFrontLL : heartbeatStheno; } - if (heartbeatEuryale == -1 || this.lastHeartbeatEuryale < heartbeatEuryale) { + if (heartbeatEuryale == -1 || this.lastHeartbeatBackLL < heartbeatEuryale) { backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); this.limelightDatas[1] = new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2); - this.lastHeartbeatEuryale = heartbeatEuryale == -1 ? this.lastHeartbeatEuryale : heartbeatEuryale; + this.lastHeartbeatBackLL = heartbeatEuryale == -1 ? this.lastHeartbeatBackLL : heartbeatEuryale; } ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); - // If our angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, - // the data is outdated (no new heartbeat), or there is no data, ignore vision updates. + // If the bot's angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, + // or for both LLs the data is outdated or has no data, ignore vision updates. if (Math.abs(Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond)) > 270 || Math.abs(velocity) > 2 // m/s - || (this.lastHeartbeatEuryale != heartbeatEuryale && this.lastHeartbeatStheno != heartbeatStheno) + || (this.lastHeartbeatBackLL != heartbeatEuryale && this.lastHeartbeatFrontLL != heartbeatStheno) || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) || (frontLLDataMT2 != null && frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE @@ -100,6 +102,7 @@ private LimelightData[] getLimelightData() { return new LimelightData[0]; } + // Allows LLs to compare data even if they have unsynced FPS / heartbeats. if (frontLLDataMT2 == null) { frontLLDataMT2 = this.limelightDatas[0].MegaTag2; } @@ -107,27 +110,33 @@ private LimelightData[] getLimelightData() { backLLDataMT2 = this.limelightDatas[1].MegaTag2; } - // More tags = more accurate. Also eliminate the other if it happens to be invalid. - if (this.lastHeartbeatStheno == heartbeatStheno + // Returns the data with the greater tag count. + // Will only return the data if it has the same heartbeat as just captured (if it doesn't, + // this means the data was retrieved from this.limelightDatas and not during this loop). + if ((!ignoreHeartbeat && this.lastHeartbeatFrontLL == heartbeatStheno) && (backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || frontLLDataMT2.tagCount > backLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if (this.lastHeartbeatEuryale == heartbeatEuryale + else if ((!ignoreHeartbeat && this.lastHeartbeatBackLL == heartbeatEuryale) && (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || backLLDataMT2.tagCount > frontLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[1] }; } - // If it's way closer, then trust only that one. 90% is a heuteristic - if (this.lastHeartbeatStheno == heartbeatStheno && frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.9) { + // Returns the data that's closer to its respective camera than 90% of the other's distance. + // 90% is a heuteristic. + if ((!ignoreHeartbeat && this.lastHeartbeatFrontLL == heartbeatStheno) + && frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.9) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if (this.lastHeartbeatEuryale == heartbeatEuryale && backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.9) { + else if ((!ignoreHeartbeat && this.lastHeartbeatBackLL == heartbeatEuryale) + && backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.9) { return new LimelightData[]{ this.limelightDatas[1] }; } - // Both have the same amount of tags, both are almost equadistant from tags + // This return statement assumes that both LLs have the same amount of tags and + // are near equadistant from one another. return this.limelightDatas; } @@ -144,8 +153,8 @@ private void optimizeLimelights() { return; } - // Avoid unnecessary optimization for a LL with no tags - // Reset any optimization that might have been done previously + // Avoid unnecessary optimization for a LL with no tags and + // reset any optimization that might have been done previously. if (limelightData.MegaTag2 == null || limelightData.MegaTag2.tagCount == 0) { LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, LimelightConstants.ALL_TAG_IDS); @@ -153,7 +162,7 @@ private void optimizeLimelights() { continue; } - // Tag filtering + // Tag filtering for nearby tags. Set nearbyTagsSet = new HashSet(); for (LimelightHelpers.RawFiducial fiducial : limelightData.MegaTag2.rawFiducials) { switch (fiducial.id) { @@ -180,7 +189,7 @@ private void optimizeLimelights() { int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); - // Downscaling closer to tags + // Downscaling closer to tags. if (limelightData.MegaTag2.avgTagDist < 1.75) { LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); } @@ -191,8 +200,9 @@ else if (limelightData.MegaTag2.avgTagDist < 2.5) { LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); } - // Smart cropping follows AprilTags - // See : https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#basic-targeting-data + // Smart cropping around on-screen AprilTags and potential nearby ones. + // For explanations of variables such as tx vs txnc, see : + // https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#basic-targeting-data. if (limelightData.MegaTag2.rawFiducials.length == 0) { LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); } @@ -201,24 +211,25 @@ else if (limelightData.MegaTag2.avgTagDist < 2.5) { LimelightHelpers.RawFiducial txncSmall = null; LimelightHelpers.RawFiducial tyncBig = null; LimelightHelpers.RawFiducial tyncSmall = null; - double targetSize = 0; + double sideLength = 0; // Finds the txnc and tync that are furthest from the crosshair - // (for largest bounding box that will include all targets on screen) + // (for largest bounding box that will include all targets on screen). for (LimelightHelpers.RawFiducial fiducial: limelightData.MegaTag2.rawFiducials) { - targetSize = Math.sqrt(fiducial.ta * LimelightConstants.FOV_AREA) / 2; + // This formula is explained below. + sideLength = Math.sqrt(fiducial.ta * LimelightConstants.FOV_AREA) / 2; - if (txncBig == null || fiducial.txnc + targetSize > txncBig.txnc) { + if (txncBig == null || fiducial.txnc + sideLength > txncBig.txnc) { txncBig = fiducial; } - if (txncSmall == null || fiducial.txnc - targetSize < txncSmall.txnc) { + if (txncSmall == null || fiducial.txnc - sideLength < txncSmall.txnc) { txncSmall = fiducial; } - if (tyncBig == null || fiducial.tync + targetSize > tyncBig.tync) { + if (tyncBig == null || fiducial.tync + sideLength > tyncBig.tync) { tyncBig = fiducial; } - if (tyncSmall == null || fiducial.tync - targetSize < tyncSmall.tync) { + if (tyncSmall == null || fiducial.tync - sideLength < tyncSmall.tync) { tyncSmall = fiducial; } } @@ -226,19 +237,19 @@ else if (limelightData.MegaTag2.avgTagDist < 2.5) { // The formulas used below create the bounding boxes around targets and work in this way : // // The position of the target (x or y) that was found to be the furthest from the principal pixel for that direction - // (largest/smallest x and largest/smallest y) - // MINUS for the smallest positions (left/bottom of the box) or PLUS for the largest positions (right/top of the box) + // (largest/smallest x and largest/smallest y). + // MINUS for the smallest positions (left/bottom of the box) or PLUS for the largest positions (right/top of the box). // The length of the side of the targets — This is found in the following way : // We know the FOV area (LimelightConstants.FOV_AREA) -> We know percentage of screen target occupies (ta) -> - // Targets are roughly squares at most angles so sqrt(target area in pixels) = side lengths + // Targets are roughly squares at most angles so sqrt(target area in pixels) = side lengths. // Which is MULTIPLIED by a function that scales with distance (further away needs larger box due // to bot movements having more impact on target position from the camera's POV) in the following way : - // 2 (heuteristic, this determines general box size) * ln(distance to target + 1) + // ` 2 (heuteristic, this determines general box size) * ln(distance to target + 1) ` // The +1 is added to the natural log to avoid a negative value for distances of less than 1 meter, // even if those are very rare. Natural log is probably not the best function for this, but it works well enough. // // Together this comes out as (be careful to follow order of operations) : - // Target Position +/- Target Length * (2 * ln(Distance + 1)) + // ` Target Position +/- Target Length * (2 * ln(Distance + 1)) ` // // In the end this is DIVIDED by HALF of the rough width or height of the FOV, // because Limelights expect cropping to be [-1.0, 1.0]. @@ -250,6 +261,10 @@ else if (limelightData.MegaTag2.avgTagDist < 2.5) { LimelightHelpers.setCropWindow( limelightData.name, + // In the x directions, 2.5x the size of the box if there are expected tags there. + // This allows the LL to lose the second tag for a few frame without cropping solely to + // the remaining one and no longer seeing the other (since crop only resets when both tags are lost). + // leftmost coordinate - 1.5 * (horizontal size of box) = a box 2.5x its original size getNearbyTagDirection(txncSmall.id) < 0 ? xSmall - 1.5 * Math.abs(xBig - xSmall) : xSmall, getNearbyTagDirection(txncBig.id) > 0 ? xBig + 1.5 * Math.abs(xBig - xSmall) : xBig, (tyncSmall.tync - Math.sqrt(tyncSmall.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(tyncBig.distToCamera + 1))) @@ -282,38 +297,39 @@ private int getNearbyTagDirection(int id) { default: return 0; } - }; + } /** * Gets the most trustworthy data from each Limelight and returns a {@link Pose2d} object. - * @return The most accurate {@link Pose2d}, or {@code null} if there is none. + * @return The most accurate {@link Pose2d}, or an empty one if there is none. + * @apiNote If MegaTag rotation cannot be trusted, it will use the odometry's current rotation. */ public Pose2d getEstimatedPose() { - LimelightData[] limelightDatas = getLimelightData(); + LimelightData[] filteredLimelightDatas = getFilteredLimelightData(true); - if (limelightDatas.length == 0) { + if (filteredLimelightDatas.length == 0) { System.err.println("getEstimatedPose() | NO LIMELIGHT DATA, DEFAULTING TO EMTPY POSE2D"); return new Pose2d(); } - else if (limelightDatas.length == 1) { + else if (filteredLimelightDatas.length == 1) { return new Pose2d( - limelightDatas[0].MegaTag2.pose.getTranslation(), - limelightDatas[0].canTrustRotation() ? - limelightDatas[0].MegaTag.pose.getRotation() : TunerConstants.DriveTrain.getState().Pose.getRotation() + filteredLimelightDatas[0].MegaTag2.pose.getTranslation(), + filteredLimelightDatas[0].canTrustRotation() ? + filteredLimelightDatas[0].MegaTag.pose.getRotation() : TunerConstants.DriveTrain.getState().Pose.getRotation() ); } else { // Average them for best accuracy return new Pose2d( // (First translation + Second translation) / 2 - limelightDatas[0].MegaTag2.pose.getTranslation().plus(limelightDatas[1].MegaTag2.pose.getTranslation()).div(2), - limelightDatas[0].canTrustRotation() ? + filteredLimelightDatas[0].MegaTag2.pose.getTranslation().plus(filteredLimelightDatas[1].MegaTag2.pose.getTranslation()).div(2), + filteredLimelightDatas[0].canTrustRotation() ? // First rotation / 2 + Second rotation / 2 // // This is done to avoid problems due to Rotation2d being [0, 360) // Ex : 180+180=0 followed by 0/2=0 when it should be 180+180=360 and 360/2=180. - limelightDatas[0].MegaTag.pose.getRotation().div(2) - .plus(limelightDatas[1].MegaTag.pose.getRotation().div(2)) : + filteredLimelightDatas[0].MegaTag.pose.getRotation().div(2) + .plus(filteredLimelightDatas[1].MegaTag.pose.getRotation().div(2)) : TunerConstants.DriveTrain.getState().Pose.getRotation() ); } @@ -322,13 +338,14 @@ else if (limelightDatas.length == 1) { // This method will be called once per scheduler run @Override public void periodic() { - // This method variably gets data in between 4-10 ms. - LimelightData[] limelightDatas = getLimelightData(); + // This method gets data in about 4 to 8 ms. + LimelightData[] filteredLimelightDatas = getFilteredLimelightData(false); - // This loop generally updates data in 6 ms, but may double or triple for no apparent reason. - for (LimelightData data : limelightDatas) { + // This loop generally updates data in about 6 ms, but may double or triple for no apparent reason. + // This causes loop overrun warnings, however, it doesn't seem to be due to inefficient code and thus can be ignored. + for (LimelightData data : filteredLimelightDatas) { if (data.canTrustRotation()) { - // Only trust rotational data + // Only trust rotational data when adding this pose. TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 0.5)); TunerConstants.DriveTrain.addVisionMeasurement( data.MegaTag.pose, @@ -336,7 +353,7 @@ public void periodic() { ); } - // Only trust positional data + // Only trust positional data when adding this pose. TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); TunerConstants.DriveTrain.addVisionMeasurement( data.MegaTag2.pose, From 5ccdfd70c1c05268927dec04cfcfe7494b12ec33 Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Tue, 30 Jul 2024 11:04:11 -0700 Subject: [PATCH 20/23] Removed PrimeNumbers.java (CANivore is fast and can handle spam) --- .../frc/robot/constants/PrimeNumbers.java | 32 ------------------- 1 file changed, 32 deletions(-) delete mode 100644 src/main/java/frc/robot/constants/PrimeNumbers.java diff --git a/src/main/java/frc/robot/constants/PrimeNumbers.java b/src/main/java/frc/robot/constants/PrimeNumbers.java deleted file mode 100644 index 1af60f2..0000000 --- a/src/main/java/frc/robot/constants/PrimeNumbers.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.constants; - -/** - * A class that stores prime numbers to be used with CAN messages so they do not overlap. - */ -public final class PrimeNumbers { - private static final int[] primeNumbers = new int[]{ - 10177, 10193, 10321, 10337, 10711, 10771, 10949, 11177, 11351, - 11393, 11443, 11821, 12157, 12203, 12277, 12377, 12409, 12473, 12619, 12763, 12821, 13187, - 13259, 13267, 13313, 13411, 14153, 14327, 14419, 14543, 14633, 14639, 14929, 15233, 15289, - 15383, 15497, 15607, 15671, 15739, 15749, 15761, 15797, 15809, 15881, 16183, 16943, 17293, - 17359, 17419, 17597, 17623, 18043, 18287, 19207, 19379, 19471, 19543, 19553, 19753, 19843, - 19961, 19963, 20029, 20117, 20477, 20521, 20789, 21067, 21121, 21247, 21317, 21419, 21491, - 21521, 21587, 21613, 21767, 21817, 22307, 22397, 22679, 22937, 23053, 23567, 23581, 23767, - 23977, 24223, 24421, 24473, 24763, 24767, 25147, 25463, 25849, 26017, 26041, 26263, 26309, - 26357, 26371, 26399, 26407, 26437, 26759, 26777, 26987, 27109, 27239, 27397, 27617, 27691, - 27733, 27883, 27961, 28151, 28393, 28463, 28537, 28541, 28751, 28807, 29333, 29453, 29501, - 29671, 29723, 30103, 30119, 30203, 30313, 30557, 31391, 31513, 32299, 32309, 32341, 32363, - 32503, 32531, 32573, 32693 - }; - - private static int primeNumberIndex = 0; - - /** - * Gets the next prime number in the array. - * @return 5-digit prime number. - * @throws ArrayIndexOutOfBoundsException when the robot is out of prime numbers. - */ - public static int getNextPrimeNumber() { - return primeNumbers[primeNumberIndex++]; - } -} From 85dc8f28527c3315f2ff27dfb712d59e206b416c Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Tue, 30 Jul 2024 21:25:16 -0700 Subject: [PATCH 21/23] Integrated VisionSubsystem with other robot code --- .pathplanner/settings.json | 4 +- src/main/java/frc/robot/RobotContainer.java | 15 +- .../robot/constants/PhysicalConstants.java | 2 +- .../java/frc/robot/vision/LimelightData.java | 26 ++- .../frc/robot/vision/VisionSubsystem.java | 161 +++++++++++------- 5 files changed, 133 insertions(+), 75 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 5039214..13015e2 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.74, - "robotLength": 0.74, + "robotWidth": 0.71, + "robotLength": 0.51, "holonomicMode": true, "pathFolders": [], "autoFolders": [], diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4cba417..701f12c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,9 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -128,10 +130,12 @@ private void configureDrivetrain() { // Burger driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); // Double Rectangle - driverController.back().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative( - // VisionSubsystem.getInstance().getEstimatedPose() - Positions.getStartingPose(PositionInitialization.MIDDLE) - ))); + driverController.back().onTrue(drivetrain.runOnce(() -> { + Pose2d estimatedPose = VisionSubsystem.getInstance().getEstimatedPose(); + if (!estimatedPose.equals(new Pose2d())) { + drivetrain.seedFieldRelative(estimatedPose); + } + })); // This looks terrible, but I can't think of a better way to do it = 2 targets. * @return Whether rotation data can be trusted. * @apiNote Dist <= 3 meters ; Angular <= 160 deg/s ; Translational <= 2. */ - public boolean canTrustRotation() { + private boolean canTrustRotation() { ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); - // 3 Meters - return this.MegaTag2.avgTagDist <= 3 + return this.MegaTag2 != null // 3 Meters + && this.MegaTag2.avgTagDist <= 3 + && this.MegaTag != null + && this.MegaTag.tagCount >= 2 && Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond) <= 160 && velocity <= 2; } + + /** + * Checks if the MegaTag2 Pose2d is within an acceptable distance of the bot's position. + * @return Whether position data can be trusted. + */ + private boolean canTrustPosition() { + return this.MegaTag2 != null + && this.MegaTag2.avgTagDist < LimelightConstants.TRUST_TAG_DISTANCE + && TunerConstants.DriveTrain.getState().Pose.getTranslation().getDistance(this.MegaTag2.pose.getTranslation()) <= 1.5; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index bbcbd64..afcad42 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.PhysicalConstants.LimelightConstants; import frc.robot.swerve.TunerConstants; @@ -58,79 +59,103 @@ private VisionSubsystem() { /** * Gets the most trustworthy data from each Limelight. Also updates {@link VisionSubsystem#limelightDatas} and heartbeats. - * @param ignoreHeartbeat - This will only ignore heartbeats for old data, meaning that this method is allowed to return stale/stored data. - * It will not ignore hearbeats when querying new data. + * @param useStored - When true, no data will be retrieved from the Limelights, stored data will be filtered instead. * @return LimelightData for each trustworthy Limelight data. * @apiNote Will theoretically stop updating data if the heartbeat resets. * However, this happens at 2e9 frames, which would take consecutive 96 days at a consistent 240 fps. */ - private LimelightData[] getFilteredLimelightData(boolean ignoreHeartbeat) { - double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); - LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); - LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, rotationDegrees, 0, 0, 0, 0, 0); - + private LimelightData[] getFilteredLimelightData(boolean useStored) { LimelightHelpers.PoseEstimate frontLLDataMT2 = null; LimelightHelpers.PoseEstimate backLLDataMT2 = null; - long heartbeatStheno = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.FRONT_APRIL_TAG_LL, "hb").getInteger(-1); - long heartbeatEuryale = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.BACK_APRIL_TAG_LL, "hb").getInteger(-1); + long heartbeatFrontLL = -1; + long heartbeatBackLL = -1; - if (heartbeatStheno == -1 || this.lastHeartbeatFrontLL < heartbeatStheno) { - frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate frontLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL); - this.limelightDatas[0] = new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2); - this.lastHeartbeatFrontLL = heartbeatStheno == -1 ? this.lastHeartbeatFrontLL : heartbeatStheno; - } + // Periodic logic + if (!useStored) { + double rotationDegrees = TunerConstants.DriveTrain.getState().Pose.getRotation().getDegrees(); + LimelightHelpers.SetRobotOrientation(LimelightConstants.FRONT_APRIL_TAG_LL, + rotationDegrees, 0, 0, 0, 0, 0 + ); + LimelightHelpers.SetRobotOrientation(LimelightConstants.BACK_APRIL_TAG_LL, + rotationDegrees, 0, 0, 0, 0, 0 + ); + + heartbeatFrontLL = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.FRONT_APRIL_TAG_LL, "hb").getInteger(-1); + heartbeatBackLL = LimelightHelpers.getLimelightNTTableEntry(LimelightConstants.BACK_APRIL_TAG_LL, "hb").getInteger(-1); - if (heartbeatEuryale == -1 || this.lastHeartbeatBackLL < heartbeatEuryale) { - backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); - LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); - this.limelightDatas[1] = new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2); - this.lastHeartbeatBackLL = heartbeatEuryale == -1 ? this.lastHeartbeatBackLL : heartbeatEuryale; - } + if (heartbeatFrontLL == -1 || this.lastHeartbeatFrontLL < heartbeatFrontLL) { + frontLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.FRONT_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate frontLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.FRONT_APRIL_TAG_LL); + this.limelightDatas[0] = new LimelightData(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLDataMT, frontLLDataMT2); + this.lastHeartbeatFrontLL = heartbeatFrontLL == -1 ? this.lastHeartbeatFrontLL : heartbeatFrontLL; + } + + if (heartbeatBackLL == -1 || this.lastHeartbeatBackLL < heartbeatBackLL) { + backLLDataMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LimelightConstants.BACK_APRIL_TAG_LL); + LimelightHelpers.PoseEstimate backLLDataMT = LimelightHelpers.getBotPoseEstimate_wpiBlue(LimelightConstants.BACK_APRIL_TAG_LL); + this.limelightDatas[1] = new LimelightData(LimelightConstants.BACK_APRIL_TAG_LL, backLLDataMT, backLLDataMT2); + this.lastHeartbeatBackLL = heartbeatBackLL == -1 ? this.lastHeartbeatBackLL : heartbeatBackLL; + } - ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); - double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); - // If the bot's angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, - // or for both LLs the data is outdated or has no data, ignore vision updates. - if (Math.abs(Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond)) > 270 - || Math.abs(velocity) > 2 // m/s - || (this.lastHeartbeatBackLL != heartbeatEuryale && this.lastHeartbeatFrontLL != heartbeatStheno) - || ((frontLLDataMT2 == null || frontLLDataMT2.tagCount == 0) - && (backLLDataMT2 == null || backLLDataMT2.tagCount == 0)) - || (frontLLDataMT2 != null && frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE - && backLLDataMT2 != null && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { - return new LimelightData[0]; + ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); + double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); + // If the bot's angular velocity is greater than 270 deg/s, translational velocity is over 2 m/s, + // or for both LLs the data is outdated or has no data, ignore vision updates. + if (Math.abs(Units.radiansToDegrees(robotChassisSpeeds.omegaRadiansPerSecond)) > 270 + || Math.abs(velocity) > 2 // m/s + || (this.lastHeartbeatBackLL != heartbeatBackLL && this.lastHeartbeatFrontLL != heartbeatFrontLL) + || ((frontLLDataMT2 != null && frontLLDataMT2.tagCount == 0) && (backLLDataMT2 != null && backLLDataMT2.tagCount == 0)) + || (frontLLDataMT2 != null && frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + && backLLDataMT2 != null && backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE)) { + return new LimelightData[0]; + } } // Allows LLs to compare data even if they have unsynced FPS / heartbeats. + // Upon startup, these may still be null, so it is important to check for them or robot code could crash. + // Also, ignore data that is older than 1 second. + double timestampNow = Timer.getFPGATimestamp(); if (frontLLDataMT2 == null) { frontLLDataMT2 = this.limelightDatas[0].MegaTag2; + + if (frontLLDataMT2 != null && Math.abs(frontLLDataMT2.timestampSeconds - timestampNow) > 1) { + frontLLDataMT2 = null; + } } if (backLLDataMT2 == null) { backLLDataMT2 = this.limelightDatas[1].MegaTag2; + + if (backLLDataMT2 != null && Math.abs(backLLDataMT2.timestampSeconds - timestampNow) > 1) { + backLLDataMT2 = null; + } + } + if (frontLLDataMT2 == null && backLLDataMT2 == null) { + return new LimelightData[0]; } // Returns the data with the greater tag count. // Will only return the data if it has the same heartbeat as just captured (if it doesn't, // this means the data was retrieved from this.limelightDatas and not during this loop). - if ((!ignoreHeartbeat && this.lastHeartbeatFrontLL == heartbeatStheno) - && (backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + if (((useStored && frontLLDataMT2 != null) || this.lastHeartbeatFrontLL == heartbeatFrontLL) + && (backLLDataMT2 == null + || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || frontLLDataMT2.tagCount > backLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if ((!ignoreHeartbeat && this.lastHeartbeatBackLL == heartbeatEuryale) - && (frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE + else if (((useStored && backLLDataMT2 != null) || this.lastHeartbeatBackLL == heartbeatBackLL) + && (frontLLDataMT2 == null + || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || backLLDataMT2.tagCount > frontLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[1] }; } // Returns the data that's closer to its respective camera than 90% of the other's distance. // 90% is a heuteristic. - if ((!ignoreHeartbeat && this.lastHeartbeatFrontLL == heartbeatStheno) + if ((!useStored && this.lastHeartbeatFrontLL == heartbeatFrontLL) && frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.9) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if ((!ignoreHeartbeat && this.lastHeartbeatBackLL == heartbeatEuryale) + else if ((!useStored && this.lastHeartbeatBackLL == heartbeatBackLL) && backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.9) { return new LimelightData[]{ this.limelightDatas[1] }; } @@ -146,11 +171,11 @@ else if ((!ignoreHeartbeat && this.lastHeartbeatBackLL == heartbeatEuryale) private void optimizeLimelights() { byte index = 0; // Used only for setting the optimized flag, so that this can be a for-each loop. for (LimelightData limelightData : this.limelightDatas) { - if (!limelightData.optimized) { - this.limelightDatas[index++].optimized = true; + if (limelightData == null || limelightData.optimized) { + return; } else { - return; + this.limelightDatas[index++].optimized = true; } // Avoid unnecessary optimization for a LL with no tags and @@ -162,6 +187,17 @@ private void optimizeLimelights() { continue; } + // Downscaling closer to tags. + if (limelightData.MegaTag2.avgTagDist < 1.5) { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); + } + else if (limelightData.MegaTag2.avgTagDist < 2) { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); + } + else { + LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); + } + // Tag filtering for nearby tags. Set nearbyTagsSet = new HashSet(); for (LimelightHelpers.RawFiducial fiducial : limelightData.MegaTag2.rawFiducials) { @@ -188,17 +224,6 @@ private void optimizeLimelights() { } int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); - - // Downscaling closer to tags. - if (limelightData.MegaTag2.avgTagDist < 1.75) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 3); - } - else if (limelightData.MegaTag2.avgTagDist < 2.5) { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 2); - } - else { - LimelightHelpers.SetFiducialDownscalingOverride(limelightData.name, 1.5f); - } // Smart cropping around on-screen AprilTags and potential nearby ones. // For explanations of variables such as tx vs txnc, see : @@ -312,18 +337,26 @@ public Pose2d getEstimatedPose() { return new Pose2d(); } else if (filteredLimelightDatas.length == 1) { + if (filteredLimelightDatas[0].MegaTag2.tagCount == 0) { + return new Pose2d(); + } + return new Pose2d( filteredLimelightDatas[0].MegaTag2.pose.getTranslation(), - filteredLimelightDatas[0].canTrustRotation() ? + filteredLimelightDatas[0].canTrustRotation ? filteredLimelightDatas[0].MegaTag.pose.getRotation() : TunerConstants.DriveTrain.getState().Pose.getRotation() ); } else { + if (filteredLimelightDatas[0].MegaTag2.tagCount == 0 || filteredLimelightDatas[1].MegaTag2.tagCount == 0) { + return new Pose2d(); + } + // Average them for best accuracy return new Pose2d( // (First translation + Second translation) / 2 filteredLimelightDatas[0].MegaTag2.pose.getTranslation().plus(filteredLimelightDatas[1].MegaTag2.pose.getTranslation()).div(2), - filteredLimelightDatas[0].canTrustRotation() ? + filteredLimelightDatas[0].canTrustRotation ? // First rotation / 2 + Second rotation / 2 // // This is done to avoid problems due to Rotation2d being [0, 360) @@ -344,21 +377,23 @@ public void periodic() { // This loop generally updates data in about 6 ms, but may double or triple for no apparent reason. // This causes loop overrun warnings, however, it doesn't seem to be due to inefficient code and thus can be ignored. for (LimelightData data : filteredLimelightDatas) { - if (data.canTrustRotation()) { + if (data.canTrustRotation) { // Only trust rotational data when adding this pose. - TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 0.5)); + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 1)); TunerConstants.DriveTrain.addVisionMeasurement( data.MegaTag.pose, data.MegaTag.timestampSeconds ); } - // Only trust positional data when adding this pose. - TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999)); - TunerConstants.DriveTrain.addVisionMeasurement( - data.MegaTag2.pose, - data.MegaTag2.timestampSeconds - ); + if (data.canTrustPosition) { + // Only trust positional data when adding this pose. + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, 9999999)); + TunerConstants.DriveTrain.addVisionMeasurement( + data.MegaTag2.pose, + data.MegaTag2.timestampSeconds + ); + } } // This method is suprprisingly efficient, generally below 1 ms. From a40eb11560914500acc0e77999022b66bcf07cdf Mon Sep 17 00:00:00 2001 From: Anonyma Date: Tue, 30 Jul 2024 22:13:58 -0700 Subject: [PATCH 22/23] Camera feed in shuffleboard --- .../frc/robot/vision/VisionSubsystem.java | 92 ++++++++++++------- 1 file changed, 60 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index afcad42..8c6dbaf 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -7,14 +7,20 @@ import java.util.Arrays; import java.util.HashSet; import java.util.List; +import java.util.Map; import java.util.Set; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants.ShuffleboardTabNames; import frc.robot.constants.PhysicalConstants.LimelightConstants; import frc.robot.swerve.TunerConstants; @@ -55,6 +61,60 @@ public static VisionSubsystem getInstance() { /** Creates a new VisionSubsystem. */ private VisionSubsystem() { super("VisionSubsystem"); + + // Shuffleboard camera feeds. + HttpCamera frontLLCamera = new HttpCamera( + LimelightConstants.FRONT_APRIL_TAG_LL, + "http://" + LimelightConstants.FRONT_APRIL_TAG_LL + ".local:5800/stream.mjpg" + ); + HttpCamera backLLCamera = new HttpCamera( + LimelightConstants.BACK_APRIL_TAG_LL, + "http://" + LimelightConstants.BACK_APRIL_TAG_LL + ".local:5800/stream.mjpg" + ); + + CameraServer.addCamera(frontLLCamera); + CameraServer.addCamera(backLLCamera); + + Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT) + .add(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLCamera) + .withWidget(BuiltInWidgets.kCameraStream) + .withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); + Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT) + .add(LimelightConstants.BACK_APRIL_TAG_LL, backLLCamera) + .withWidget(BuiltInWidgets.kCameraStream) + .withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); + } + + // This method will be called once per scheduler run + @Override + public void periodic() { + // This method gets data in about 4 to 8 ms. + LimelightData[] filteredLimelightDatas = getFilteredLimelightData(false); + + // This loop generally updates data in about 6 ms, but may double or triple for no apparent reason. + // This causes loop overrun warnings, however, it doesn't seem to be due to inefficient code and thus can be ignored. + for (LimelightData data : filteredLimelightDatas) { + if (data.canTrustRotation) { + // Only trust rotational data when adding this pose. + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 1)); + TunerConstants.DriveTrain.addVisionMeasurement( + data.MegaTag.pose, + data.MegaTag.timestampSeconds + ); + } + + if (data.canTrustPosition) { + // Only trust positional data when adding this pose. + TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, 9999999)); + TunerConstants.DriveTrain.addVisionMeasurement( + data.MegaTag2.pose, + data.MegaTag2.timestampSeconds + ); + } + } + + // This method is suprprisingly efficient, generally below 1 ms. + optimizeLimelights(); } /** @@ -367,36 +427,4 @@ else if (filteredLimelightDatas.length == 1) { ); } } - - // This method will be called once per scheduler run - @Override - public void periodic() { - // This method gets data in about 4 to 8 ms. - LimelightData[] filteredLimelightDatas = getFilteredLimelightData(false); - - // This loop generally updates data in about 6 ms, but may double or triple for no apparent reason. - // This causes loop overrun warnings, however, it doesn't seem to be due to inefficient code and thus can be ignored. - for (LimelightData data : filteredLimelightDatas) { - if (data.canTrustRotation) { - // Only trust rotational data when adding this pose. - TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(9999999, 9999999, 1)); - TunerConstants.DriveTrain.addVisionMeasurement( - data.MegaTag.pose, - data.MegaTag.timestampSeconds - ); - } - - if (data.canTrustPosition) { - // Only trust positional data when adding this pose. - TunerConstants.DriveTrain.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, 9999999)); - TunerConstants.DriveTrain.addVisionMeasurement( - data.MegaTag2.pose, - data.MegaTag2.timestampSeconds - ); - } - } - - // This method is suprprisingly efficient, generally below 1 ms. - optimizeLimelights(); - } } From 0a4c1259612bd57561aed215eb336af10523f6bd Mon Sep 17 00:00:00 2001 From: Arrowbotics3482 Date: Wed, 31 Jul 2024 10:46:53 -0700 Subject: [PATCH 23/23] Fixed camera feeds on shuffleboard --- src/main/java/frc/robot/vision/VisionSubsystem.java | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 8c6dbaf..105b922 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -71,9 +71,6 @@ private VisionSubsystem() { LimelightConstants.BACK_APRIL_TAG_LL, "http://" + LimelightConstants.BACK_APRIL_TAG_LL + ".local:5800/stream.mjpg" ); - - CameraServer.addCamera(frontLLCamera); - CameraServer.addCamera(backLLCamera); Shuffleboard.getTab(ShuffleboardTabNames.DEFAULT) .add(LimelightConstants.FRONT_APRIL_TAG_LL, frontLLCamera) @@ -196,13 +193,13 @@ private LimelightData[] getFilteredLimelightData(boolean useStored) { // Returns the data with the greater tag count. // Will only return the data if it has the same heartbeat as just captured (if it doesn't, // this means the data was retrieved from this.limelightDatas and not during this loop). - if (((useStored && frontLLDataMT2 != null) || this.lastHeartbeatFrontLL == heartbeatFrontLL) + if ((frontLLDataMT2 != null && (useStored || this.lastHeartbeatFrontLL == heartbeatFrontLL)) && (backLLDataMT2 == null || backLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || frontLLDataMT2.tagCount > backLLDataMT2.tagCount)) { return new LimelightData[]{ this.limelightDatas[0] }; } - else if (((useStored && backLLDataMT2 != null) || this.lastHeartbeatBackLL == heartbeatBackLL) + else if ((backLLDataMT2 != null && (useStored || this.lastHeartbeatBackLL == heartbeatBackLL)) && (frontLLDataMT2 == null || frontLLDataMT2.avgTagDist > LimelightConstants.TRUST_TAG_DISTANCE || backLLDataMT2.tagCount > frontLLDataMT2.tagCount)) {