diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..13015e2 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.71, + "robotLength": 0.51, + "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/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 { 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/Robot.java b/src/main/java/frc/robot/Robot.java index d851568..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 @@ -27,10 +29,11 @@ public class Robot extends TimedRobot { @Override public void robotInit() { // 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(); @@ -73,7 +76,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 7064644..701f12c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,17 +4,34 @@ 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 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; 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 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.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. @@ -37,6 +54,9 @@ public static RobotContainer getInstance() { } private final SendableChooser autoChooser; + // Position chooser + 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 +66,9 @@ public static RobotContainer getInstance() { 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(); @@ -54,17 +76,115 @@ public RobotContainer() { configureDriverBindings(); 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); + 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", this.autoChooser); } - /** Creates instances of each subsystem so periodic runs */ - private void initializeSubsystems() { + /** + * 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); + + Trigger rightBumper = driverController.rightBumper(); + + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + 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) + ); + + driverController.x().whileTrue(drivetrain.applyRequest(() -> brake)); + driverController.y().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection( + 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())); + // Double Rectangle + 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 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(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( + (Integer angle, Double[] speeds) -> driverController.pov(angle).whileTrue( + drivetrain.applyRequest(() -> fieldCentricMove.withVelocityX(speeds[0]).withVelocityY(speeds[1])) + ) + ); + } + + drivetrain.registerTelemetry(logger::telemeterize); + // Position chooser + 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) -> + 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() { + VisionSubsystem.getInstance(); } /** Register all NamedCommands for PathPlanner use */ @@ -78,13 +198,18 @@ 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 */ 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(); + // })); } /** @@ -92,6 +217,7 @@ private void configureOperatorBindings() { * @return The command to run in autonomous */ public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return new PathPlannerAuto("Test1"); + // return autoChooser.getSelected(); } } \ No newline at end of file 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/constants/PhysicalConstants.java b/src/main/java/frc/robot/constants/PhysicalConstants.java index b30d1bf..b46e57d 100644 --- a/src/main/java/frc/robot/constants/PhysicalConstants.java +++ b/src/main/java/frc/robot/constants/PhysicalConstants.java @@ -6,22 +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"; - /** CAN ID for the Pigeon2. */ - public static final int GYRO_ID = 1; } /** 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. This is measured from tag to camera.*/ + public static final int TRUST_TAG_DISTANCE = 4; + + /** 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"; + + /** 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 }; + + // 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/constants/Positions.java b/src/main/java/frc/robot/constants/Positions.java index 5dff437..b45f512 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; @@ -11,90 +12,94 @@ 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. */ public final class Positions { - // TODO : redo this file, because it's a freaking mess + /** + * 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 + ; + } /** 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(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(120))))), 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(0))), + Map.entry(PositionInitialization.BOTTOM, new Pose2d(new Translation2d(15.8, 4.50), Rotation2d.fromDegrees(60))))) ); - /** Names used to organize pathfinding positions in {@link Positions#PATHFIND_POSITIONS}. */ - public static enum PathfindingPosition { - SPEAKER_TOP, - SPEAKER_MIDDLE, - SPEAKER_BOTTOM, - AMP + /** + * 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) { + System.out.println("getStartingPose() | Using LIMELIGHT data"); + return VisionSubsystem.getInstance().getEstimatedPose(); + } + + 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()); + } } 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++]; - } -} 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..9babe05 --- /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), // 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 + 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..717eaa6 --- /dev/null +++ b/src/main/java/frc/robot/swerve/Telemetry.java @@ -0,0 +1,122 @@ +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 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 */ + 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"); + 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(); + 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..1c1099b --- /dev/null +++ b/src/main/java/frc/robot/swerve/TunerConstants.java @@ -0,0 +1,143 @@ +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; +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. // TODO : Tune when the bot is done + + // 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; // 20.66 + + // Theoretical free speed (m/s) at 12v applied output; + // 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; + // 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 = "ctre"; + 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 Pigeon2Configuration kPigeonConfigs = new Pigeon2Configuration() + .withMountPose(new MountPoseConfigs().withMountPoseYaw(0)); + + private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(kPigeonId) + .withCANbusName(kCANbusName) + .withPigeon2Configs(kPigeonConfigs); + + 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.RemoteCANcoder) + .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); +} 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..947e4b1 --- /dev/null +++ b/src/main/java/frc/robot/vision/LimelightData.java @@ -0,0 +1,64 @@ +package frc.robot.vision; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.PhysicalConstants.LimelightConstants; +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. + *

MT rotation should not be combined with MT2 pose, because their timestamps may differ. + */ +public class LimelightData { + public final String name; + public final LimelightHelpers.PoseEstimate MegaTag; + public final LimelightHelpers.PoseEstimate MegaTag2; + public final boolean canTrustRotation; + public final boolean canTrustPosition; + /** Flag set after optimization to avoid re-optimizing data twice in a row on low FPS. */ + public boolean optimized; + + /** + * Creates a new LimelightData object. + * @param name - The name of this Limelight. + * @param MegaTag data. + * @param MegaTag2 data. + */ + public LimelightData(String name, LimelightHelpers.PoseEstimate MegaTag, LimelightHelpers.PoseEstimate MegaTag2) { + this.name = name; + this.MegaTag = MegaTag; + this.MegaTag2 = MegaTag2; + this.optimized = false; + + this.canTrustRotation = canTrustRotation(); + this.canTrustPosition = canTrustPosition(); + } + + /** + * Checks if the average tag distance and bot's rotational and translational velocities + * are reasonable for trusting rotation data, as well as MegaTag having >= 2 targets. + * @return Whether rotation data can be trusted. + * @apiNote Dist <= 3 meters ; Angular <= 160 deg/s ; Translational <= 2. + */ + private boolean canTrustRotation() { + ChassisSpeeds robotChassisSpeeds = TunerConstants.DriveTrain.getCurrentRobotChassisSpeeds(); + double velocity = Math.sqrt(Math.pow(robotChassisSpeeds.vxMetersPerSecond, 2) + Math.pow(robotChassisSpeeds.vyMetersPerSecond, 2)); + 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/LimelightHelpers.java b/src/main/java/frc/robot/vision/LimelightHelpers.java new file mode 100644 index 0000000..2d1daa2 --- /dev/null +++ b/src/main/java/frc/robot/vision/LimelightHelpers.java @@ -0,0 +1,1269 @@ +//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]); + } + + + 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..105b922 --- /dev/null +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -0,0 +1,427 @@ +// 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 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; + +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; + } + + /** 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 lastHeartbeatFrontLL = 0; + /** Last heartbeat of the back LL (updated every frame) */ + 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); + 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 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" + ); + + 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(); + } + + /** + * Gets the most trustworthy data from each Limelight. Also updates {@link VisionSubsystem#limelightDatas} and heartbeats. + * @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 useStored) { + LimelightHelpers.PoseEstimate frontLLDataMT2 = null; + LimelightHelpers.PoseEstimate backLLDataMT2 = null; + long heartbeatFrontLL = -1; + long heartbeatBackLL = -1; + + // 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 (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 != 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 ((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 ((backLLDataMT2 != null && (useStored || 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 ((!useStored && this.lastHeartbeatFrontLL == heartbeatFrontLL) + && frontLLDataMT2.avgTagDist < backLLDataMT2.avgTagDist * 0.9) { + return new LimelightData[]{ this.limelightDatas[0] }; + } + else if ((!useStored && this.lastHeartbeatBackLL == heartbeatBackLL) + && backLLDataMT2.avgTagDist < frontLLDataMT2.avgTagDist * 0.9) { + return new LimelightData[]{ this.limelightDatas[1] }; + } + + // This return statement assumes that both LLs have the same amount of tags and + // are near equadistant from one another. + return this.limelightDatas; + } + + /** + * A helper method used to optimize Limelight FPS. + */ + 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 == null || limelightData.optimized) { + return; + } + else { + this.limelightDatas[index++].optimized = true; + } + + // 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); + LimelightHelpers.setCropWindow(limelightData.name, -1, 1, -1, 1); + 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) { + switch (fiducial.id) { + case 1: + case 2: + nearbyTagsSet.addAll(this.BLUE_SOURCE); + case 3: + case 4: + nearbyTagsSet.addAll(this.RED_SPEAKER); + case 5: + nearbyTagsSet.addAll(this.RED_AMP); + case 6: + nearbyTagsSet.addAll(this.BLUE_AMP); + case 7: + case 8: + nearbyTagsSet.addAll(this.BLUE_SPEAKER); + case 9: + case 10: + nearbyTagsSet.addAll(this.RED_SOURCE); + default: // 11, 12, 13, 14, 15, and 16 have no relevant tags near them. + nearbyTagsSet.add(fiducial.id); + } + } + int[] nearbyTagsArray = nearbyTagsSet.stream().mapToInt(i -> i).toArray(); + LimelightHelpers.SetFiducialIDFiltersOverride(limelightData.name, nearbyTagsArray); + + // 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); + } + else { + LimelightHelpers.RawFiducial txncBig = null; + LimelightHelpers.RawFiducial txncSmall = null; + LimelightHelpers.RawFiducial tyncBig = null; + LimelightHelpers.RawFiducial tyncSmall = null; + 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 (LimelightHelpers.RawFiducial fiducial: limelightData.MegaTag2.rawFiducials) { + // This formula is explained below. + sideLength = Math.sqrt(fiducial.ta * LimelightConstants.FOV_AREA) / 2; + + if (txncBig == null || fiducial.txnc + sideLength > txncBig.txnc) { + txncBig = fiducial; + } + if (txncSmall == null || fiducial.txnc - sideLength < txncSmall.txnc) { + txncSmall = fiducial; + } + + if (tyncBig == null || fiducial.tync + sideLength > tyncBig.tync) { + tyncBig = fiducial; + } + if (tyncSmall == null || fiducial.tync - sideLength < tyncSmall.tync) { + tyncSmall = fiducial; + } + } + + // 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 : + // ` 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)) ` + // + // 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) * (2 * Math.log(txncSmall.distToCamera + 1))) + / (LimelightConstants.FOV_X / 2); + double xBig = (txncBig.txnc + Math.sqrt(txncBig.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(txncBig.distToCamera + 1))) + / (LimelightConstants.FOV_X / 2); + + 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))) + / (LimelightConstants.FOV_Y / 2), + (tyncBig.tync + Math.sqrt(tyncBig.ta * LimelightConstants.FOV_AREA) * (2 * Math.log(tyncSmall.distToCamera + 1))) + / (LimelightConstants.FOV_Y / 2) + ); + } + } + } + + /** + * 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. + * @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[] filteredLimelightDatas = getFilteredLimelightData(true); + + if (filteredLimelightDatas.length == 0) { + System.err.println("getEstimatedPose() | NO LIMELIGHT DATA, DEFAULTING TO EMTPY POSE2D"); + 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].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 ? + // 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. + filteredLimelightDatas[0].MegaTag.pose.getRotation().div(2) + .plus(filteredLimelightDatas[1].MegaTag.pose.getRotation().div(2)) : + TunerConstants.DriveTrain.getState().Pose.getRotation() + ); + } + } +}