From 178359341c84ab4567569f7207ece6f76ffec317 Mon Sep 17 00:00:00 2001 From: Team2890HawkCollective <2890frc@gmail.com> Date: Fri, 20 Feb 2026 17:09:46 -0500 Subject: [PATCH] Added all 4 cameras --- src/main/java/frc/robot/Constants.java | 63 +-- src/main/java/frc/robot/Robot.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 437 ++++++++++-------- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 6 +- .../robot/subsystems/TargetingSubsystems.java | 358 +++++++------- .../swervedrive/SwerveSubsystem.java | 2 +- .../robot/subsystems/swervedrive/Vision.java | 431 ++++++++--------- 8 files changed, 657 insertions(+), 655 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 639b17e..e9c5391 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,11 +4,16 @@ package frc.robot; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; @@ -90,13 +95,6 @@ public final class Constants { public static void getIndexerAndRampMotorRPM() { INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(1000); } - - public static final int LEFT_ACTUATOR_PWM_PORT = 1; - public static final int RIGHT_ACTUATOR_PWM_PORT = 3; - - public static double DESIRED_POTENTIOMETER_DISTANCE; - - // TO DO: need to equation that calculates desired potentiometer distance } public static class IntakeConstants { @@ -111,7 +109,7 @@ public final class Constants { public static final int INTAKE_WHEELS_MOTOR_ID = 50; public static final int INTAKE_ROTATOR_MOTOR_ID = 51; public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535; - public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744; + public static final double INTAKE_MIDDLE_ENCODER_VALUE = 1.2550222873687744; public static final double INTAKE_RETRACT_ENCODER_VALUE = 0; public static class IntakeRotatorPID { @@ -131,34 +129,41 @@ public final class Constants { } - public static class LimeLight { - - public static final String LIMELIGHT_NAME = "limelight"; - - // public static final int[] ALL_REEF_APRILTAGS = { 6, 7, 8, 9, 10, 11, 17, 18, - // 19, 20, 21, 22 }; - - public static final AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = AprilTagFieldLayout - .loadField(AprilTagFields.k2026RebuiltAndymark); - - public static final double BUMPER_WIDTH = Units.inchesToMeters(0.0); // Get This Value // Original: 2.75 - // public static final double ROBOT_WIDTH = Units.inchesToMeters(30 + - // BUMPER_WIDTH); // Tis a square, don't need this - public static final double ROBOT_SIDE_LENGTH = Units.inchesToMeters(27); - // public static final Transform2d HALF_ROBOT = new - // Transform2d(ROBOT_SIDE_LENGTH / 3.0, 0, new Rotation2d()); - - public static double LIMELIGHT_TY; - } - public static class TargetingConstants { public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90)); public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90)); + + public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); + public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); + public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); + public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); + + public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0)); + public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0)); + public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0)); + public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0)); + + public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), + ORANGE_ROBOT_TO_CAM); + public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), + BLACK_ROBOT_TO_CAM); + public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), + RED_ROBOT_TO_CAM); + public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), + PURPLE_ROBOT_TO_CAM); } public static class ClimberConstants { public static final int CLIMB_MOTOR_ID = 60; - public static final int RATCHET_PWM_PORT = 0; + public static final int RATCHET_PWM_PORT = 9; public static final double RATCHET_UNLOCK_ANGLE = 0; public static final double RATCHET_LOCK_ANGLE = 180; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0704b9b..f7ed128 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -89,11 +89,20 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); + // Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); Constants.IntakeConstants.getIntakeWheelsSpeed(); Constants.ShooterConstants.getShooterVelocity(); Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0); - //distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag()); + // distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag()); + TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM, + Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); + TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM, + Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); + TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM, + Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); + TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM, + Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); + } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a80085..138cc6c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,9 @@ import java.io.File; import java.lang.annotation.Target; import java.util.concurrent.atomic.AtomicBoolean; import java.util.function.DoubleSupplier; + +import javax.lang.model.util.ElementScanner14; + import frc.robot.subsystems.TargetingSubsystems; import swervelib.SwerveInputStream; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -48,230 +51,254 @@ import frc.robot.subsystems.ClimberSubsystem; */ public class RobotContainer { - // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - // The robot's subsystems and commands are defined here... - private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), - "swerve/neo")); + // Replace with CommandPS4Controller or CommandJoystick if needed + final CommandXboxController driverXbox = new CommandXboxController(0); + // The robot's subsystems and commands are defined here... + private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), + "swerve/neo")); - // Establish a Sendable Chooser that will be able to be sent to the - // SmartDashboard, allowing selection of desired auto - private final SendableChooser autoChooser; + // Establish a Sendable Chooser that will be able to be sent to the + // SmartDashboard, allowing selection of desired auto + private final SendableChooser autoChooser; - private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); - // private final TargetingSubsystems m_TargetingSubsystems = new - // TargetingSubsystems(); - private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); - private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); + private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + // private final TargetingSubsystems m_TargetingSubsystems = new + // TargetingSubsystems(); + private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled - * by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); + /** + * Converts driver input into a field-relative ChassisSpeeds that is controlled + * by angular velocity. + */ + SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative - * input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX, - driverXbox::getRightY) - .headingWhile(true); + /** + * Clone's the angular velocity input stream and converts it to a fieldRelative + * input stream. + */ + SwerveInputStream driveDirectAngle = driveAngularVelocity.copy() + .withControllerHeadingAxis(driverXbox::getRightX, + driverXbox::getRightY) + .headingWhile(true); - /** - * Clone's the angular velocity input stream and converts it to a robotRelative - * input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false) - .allianceRelativeControl(true); + /** + * Clone's the angular velocity input stream and converts it to a robotRelative + * input stream. + */ + SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false) + .allianceRelativeControl(true); - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis( - 2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() - .withControllerHeadingAxis(() -> Math.sin( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2), - () -> Math.cos( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2)) - .headingWhile(true) - .translationHeadingOffset(true) - .translationHeadingOffset(Rotation2d.fromDegrees( - 0)); + SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX()) + .withControllerRotationAxis(() -> driverXbox.getRawAxis( + 2)) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + // Derive the heading axis with math! + SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() + .withControllerHeadingAxis(() -> Math.sin( + driverXbox.getRawAxis( + 2) * + Math.PI) + * + (Math.PI * + 2), + () -> Math.cos( + driverXbox.getRawAxis( + 2) * + Math.PI) + * + (Math.PI * + 2)) + .headingWhile(true) + .translationHeadingOffset(true) + .translationHeadingOffset(Rotation2d.fromDegrees( + 0)); - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the trigger bindings - configureBindings(); - DriverStation.silenceJoystickConnectionWarning(true); + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the trigger bindings + configureBindings(); + DriverStation.silenceJoystickConnectionWarning(true); - // Create the NamedCommands that will be used in PathPlanner - NamedCommands.registerCommand("test", Commands.print("I EXIST")); + // Create the NamedCommands that will be used in PathPlanner + NamedCommands.registerCommand("test", Commands.print("I EXIST")); - // Have the autoChooser pull in all PathPlanner autos as options - autoChooser = AutoBuilder.buildAutoChooser(); + // Have the autoChooser pull in all PathPlanner autos as options + autoChooser = AutoBuilder.buildAutoChooser(); - // Set the default auto (do nothing) - autoChooser.setDefaultOption("Do Nothing", Commands.none()); + // Set the default auto (do nothing) + autoChooser.setDefaultOption("Do Nothing", Commands.none()); - // Add a simple auto option to have the robot drive forward for 1 second then - // stop - autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1)); + // Add a simple auto option to have the robot drive forward for 1 second then + // stop + autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1)); - // Put the autoChooser on the SmartDashboard - SmartDashboard.putData("Auto Chooser", autoChooser); - - } - - /** - * Use this method to define your trigger->command mappings. Triggers can be - * created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with - * an arbitrary predicate, or via the - * named factories in - * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses - * for - * {@link CommandXboxController - * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} - * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick - * Flight joysticks}. - */ - private void configureBindings() { - Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngle); - Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); - Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); - Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngleKeyboard); - - driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand()) - .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); - driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()) - .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); - - // command for - // full shooting system including linear actuators - driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand() - .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); - - driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); - - driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand()); - driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand()); - driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); - driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); - driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true)); - driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false)); - - - // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); - driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand())); - // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), - // () -> -driverXbox.getLeftX())); - if (driverXbox.getRightY() < -0.4) { - m_ClimberSubsystem.liftRobotCommand(); - } else if (driverXbox.getRightY() > 0.4) { - m_ClimberSubsystem.lowerRobotCommand(); - } else { - m_ClimberSubsystem.stopClimberCommand(); - } - - // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE)); - - if (RobotBase.isSimulation()) { - drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); - } else { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - } - - if (Robot.isSimulation()) { - Pose2d target = new Pose2d(new Translation2d(1, 4), - Rotation2d.fromDegrees(90)); - // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target); - driveDirectAngleKeyboard.driveToPose(() -> target, - new ProfiledPIDController(5, - 0, - 0, - new Constraints(5, 2)), - new ProfiledPIDController(5, - 0, - 0, - new Constraints(Units.degreesToRadians(360), - Units.degreesToRadians(180)))); - driverXbox.start() - .onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); - driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); - driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true), - () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); - - // driverXbox.b().whileTrue( - // drivebase.driveToPose( - // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) - // ); + // Put the autoChooser on the SmartDashboard + SmartDashboard.putData("Auto Chooser", autoChooser); } - if (DriverStation.isTest()) { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above! - driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.back().whileTrue(drivebase.centerModulesCommand()); - driverXbox.leftBumper().onTrue(Commands.none()); - driverXbox.rightBumper().onTrue(Commands.none()); - } else { - driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); - driverXbox.start().whileTrue(Commands.none()); - driverXbox.back().whileTrue(Commands.none()); - driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.rightBumper().onTrue(Commands.none()); + /** + * Use this method to define your trigger->command mappings. Triggers can be + * created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with + * an arbitrary predicate, or via the + * named factories in + * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses + * for + * {@link CommandXboxController + * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} + * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick + * Flight joysticks}. + */ + private void configureBindings() { + Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); + Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); + Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); + Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngle); + Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); + Command driveFieldOrientedAnglularVelocityKeyboard = drivebase + .driveFieldOriented(driveAngularVelocityKeyboard); + Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngleKeyboard); + + driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand()) + .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); + driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()) + .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); + + // command for + // full shooting system including linear actuators + driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand() + .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); + + driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + + driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()) + .onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()) + .onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); + driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); + driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true)); + driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false)); + driverXbox.rightStick().onTrue(climbCommand()); + + // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); + driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand() + .andThen(m_IntakeSubsystem.deployintakeCommand())); + // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), + // () -> -driverXbox.getLeftX())); + if (driverXbox.getRightY() < -0.4) { + m_ClimberSubsystem.liftRobotCommand(); + } else if (driverXbox.getRightY() > 0.4) { + m_ClimberSubsystem.lowerRobotCommand(); + } else { + m_ClimberSubsystem.stopClimberCommand(); + } + + // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE)); + + if (RobotBase.isSimulation()) { + drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); + } else { + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + } + + if (Robot.isSimulation()) { + Pose2d target = new Pose2d(new Translation2d(1, 4), + Rotation2d.fromDegrees(90)); + // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target); + driveDirectAngleKeyboard.driveToPose(() -> target, + new ProfiledPIDController(5, + 0, + 0, + new Constraints(5, 2)), + new ProfiledPIDController(5, + 0, + 0, + new Constraints(Units.degreesToRadians(360), + Units.degreesToRadians(180)))); + driverXbox.start() + .onTrue(Commands.runOnce(() -> drivebase + .resetOdometry(new Pose2d(3, 3, new Rotation2d())))); + driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); + driverXbox.button(2) + .whileTrue(Commands.runEnd( + () -> driveDirectAngleKeyboard.driveToPoseEnabled(true), + () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); + + // driverXbox.b().whileTrue( + // drivebase.driveToPose( + // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) + // ); + + } + if (DriverStation.isTest()) { + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command + // above! + + driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.back().whileTrue(drivebase.centerModulesCommand()); + driverXbox.leftBumper().onTrue(Commands.none()); + driverXbox.rightBumper().onTrue(Commands.none()); + } else { + driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); + driverXbox.start().whileTrue(Commands.none()); + driverXbox.back().whileTrue(Commands.none()); + driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.rightBumper().onTrue(Commands.none()); + } + } - } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Pass in the selected auto from the SmartDashboard as our desired autnomous + // commmand + return autoChooser.getSelected(); + } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // Pass in the selected auto from the SmartDashboard as our desired autnomous - // commmand - return autoChooser.getSelected(); - } + public void setMotorBrake(boolean brake) { + drivebase.setMotorBrake(brake); + } - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } + public SwerveSubsystem getSwerveDrive() { + return drivebase; + } - public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( - // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), - m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + public Command climbCommand() { + if (driverXbox.getRightY() > -0.5) { + return m_ClimberSubsystem.lowerRobotCommand(); + } else if (driverXbox.getRightX() < 0.5) { + return m_ClimberSubsystem.liftRobotCommand(); + } else + return m_ClimberSubsystem.stopClimberCommand(); + + } + + public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( + // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), + m_ShooterSubsystem.shootFuelCommand(), + m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ef52bd4..9f48536 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -55,7 +55,7 @@ public class IntakeSubsystem extends SubsystemBase { } public void stopIntakeMotor() { - intakeWheelsMotor.set(0); + intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity); } public Command startIntakeMotorCommand() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index efcfa7e..eba0ce2 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -96,7 +96,7 @@ public class ShooterSubsystem extends SubsystemBase { public void setShooterMotorsRPM() { centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); - rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + //rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); } public double getShooterMotorRPM() { @@ -127,10 +127,12 @@ public class ShooterSubsystem extends SubsystemBase { public void stopShooters() { + centerShooterMotor.set(0); leftShooterMotor.set(0); - rightShooterMotor.set(0); + //rightShooterMotor.set(0); indexerAndRampMotor.set(0); + } public Command stopShooterCommand() { diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java index 4b2174f..e4ba6c4 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java @@ -1,182 +1,176 @@ -package frc.robot.subsystems; - -import java.util.List; -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonTrackedTarget; - -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPoint; -import com.pathplanner.lib.path.RotationTarget; -import com.pathplanner.lib.path.Waypoint; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import frc.robot.Constants; -import frc.robot.LimelightHelpers; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.RobotContainer; -import frc.robot.Constants; - -public class TargetingSubsystems extends SubsystemBase { - - PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera"); - Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); - PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - BACK_LEFT_CAMERA_OFFSETS); - PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001); - - public TargetingSubsystems() { - photonAimPIDController.enableContinuousInput(-180, 180); - } - - Pose2d currentRobotPose; - - public List rightClimbWaypoints; - - public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) { - GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation()); - PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0); - currentRobotPose = swerveDrive.getPose(); - rightClimbWaypoints = PathPlannerPath.waypointsFromPoses( - currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE); - - PathPlannerPath goToClimbPath = new PathPlannerPath(rightClimbWaypoints, goToClimbConstraints, null, - goalEndState); - goToClimbPath.preventFlipping = true; - - return swerveDrive.getAutonomousCommand("goToClimbPath"); - } - - public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) { - return new RunCommand(() -> { - currentRobotPose = swerveDrive.getPose(); - - Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); - - double xError = errorFromDesiredPose.getX(); - double yError = errorFromDesiredPose.getY(); - double angleError = errorFromDesiredPose.getRotation().getRadians(); - - PIDController xController = new PIDController(1.5, 0, 0); - PIDController yController = new PIDController(1.5, 0, 0); - PIDController angleController = new PIDController(3.0, 0, 0); - - angleController.enableContinuousInput(-Math.PI, Math.PI); - - double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX()); - double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY()); - double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(), - desiredPose.getRotation().getRadians()); - - swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true); - }, swerveDrive); - } - - Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { - return new RunCommand(() -> { - double rot = 0.0; - var result = photonVision.getLatestResult(); - if (result.hasTargets()) { - double yawError = result.getBestTarget().getYaw(); - rot = photonAimPIDController.calculate(yawError, 0); - } - - rot = MathUtil.clamp(rot, -3.0, 3.0); - - swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1, - driverXbox.getLeftX() * -1), rot, true); - }, swerveDrive); -} - - - public PhotonPoseEstimator getPhotonPoseEstimator() { - return photonEstimator; - } - - // static public NetworkTable table = - // NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME); - // static public NetworkTableEntry ty = table.getEntry("ty"); - // static double targetOffsetAngle_Vertical = ty.getDouble(0.0); - - // how many degrees back is your limelight rotated from perfectly vertical? - static double limelightMountAngleDegrees = 25.0; - - // distance from the center of the Limelight lens to the floor - static double limelightLensHeightInches = 27.5; - - // distance from the target to the floor - static double goalHeightInches = 44; - - static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY; - static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); - - // calculate distance - static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) - / Math.tan(angleToGoalRadians); - - public static double getDistanceFromAprilTag() { - angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY; - angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); - distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) - / Math.tan(angleToGoalRadians); - return distanceFromLimelightToGoalInches; - } - - public void updateRobotPose(SwerveSubsystem swerveDrive) { - Optional result = photonEstimator.update(photonVision.getLatestResult()); - - if (result.isPresent()) { - EstimatedRobotPose estimatedPose = result.get(); - swerveDrive.getSwerveDrive() - .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); - } - } - @Override - public void periodic() { - - /* - * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", - * photonVision.getLatestResult().getBestTarget().getYaw()); - * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value", - * photonVision.getLatestResult().getBestTarget().getPitch()); - * Shuffleboard.getTab("Vision").add("Limelight TX Value", - * LimelightHelpers.getTX("limelight")); - * Shuffleboard.getTab("Vision").add("Limelight April Tag ID", - * LimelightHelpers.getFiducialID("limelight")); - * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null); - * Shuffleboard.getTab("Vision").addCamera("Photon", - * "Arducam_OV9281_USB_Camera", - * "http://photonvision.local:5800"); - */ - } -} - +package frc.robot.subsystems; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.path.RotationTarget; +import com.pathplanner.lib.path.Waypoint; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.Constants; +import frc.robot.LimelightHelpers; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.RobotContainer; +import frc.robot.Constants; + +public class TargetingSubsystems extends SubsystemBase { + + PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001); + + public TargetingSubsystems() { + photonAimPIDController.enableContinuousInput(-180, 180); + } + + Pose2d currentRobotPose; + + public List pathWaypoints; + + public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) { + GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation()); + PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0); + currentRobotPose = swerveDrive.getPose(); + pathWaypoints = PathPlannerPath.waypointsFromPoses( + currentRobotPose, desiredPose); + + PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null, + goalEndState); + goToDesiredPose.preventFlipping = true; + + return swerveDrive.getAutonomousCommand("goToDesiredPose"); + } + + public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) { + return new RunCommand(() -> { + currentRobotPose = swerveDrive.getPose(); + + Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); + + double xError = errorFromDesiredPose.getX(); + double yError = errorFromDesiredPose.getY(); + double angleError = errorFromDesiredPose.getRotation().getRadians(); + + PIDController xController = new PIDController(1.5, 0, 0); + PIDController yController = new PIDController(1.5, 0, 0); + PIDController angleController = new PIDController(3.0, 0, 0); + + angleController.enableContinuousInput(-Math.PI, Math.PI); + + double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX()); + double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY()); + double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(), + desiredPose.getRotation().getRadians()); + + swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true); + }, swerveDrive); + } + + Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { + return new RunCommand(() -> { + double rot = 0.0; + var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult(); + if (result.hasTargets()) { + double yawError = result.getBestTarget().getYaw(); + rot = photonAimPIDController.calculate(yawError, 0); + } + + rot = MathUtil.clamp(rot, -3.0, 3.0); + + swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1, + driverXbox.getLeftX() * -1), rot, true); + }, swerveDrive); + } + + public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) { + return poseEstimator; + } + + /* // static public NetworkTable table = + // NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME); + // static public NetworkTableEntry ty = table.getEntry("ty"); + // static double targetOffsetAngle_Vertical = ty.getDouble(0.0); + + // how many degrees back is your limelight rotated from perfectly vertical? + static double limelightMountAngleDegrees = 25.0; + + // distance from the center of the Limelight lens to the floor + static double limelightLensHeightInches = 27.5; + + // distance from the target to the floor + static double goalHeightInches = 44; + + static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY; + static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); + + // calculate distance + static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) + / Math.tan(angleToGoalRadians); + + public static double getDistanceFromAprilTag() { + angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY; + angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); + distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) + / Math.tan(angleToGoalRadians); + return distanceFromLimelightToGoalInches; + } + */ + + public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) { + Optional result = poseEstimator.update(camera.getLatestResult()); + + if (result.isPresent()) { + EstimatedRobotPose estimatedPose = result.get(); + swerveDrive.getSwerveDrive() + .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); + } + } + + @Override + public void periodic() { + /* + * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", + * photonVision.getLatestResult().getBestTarget().getYaw()); + * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value", + * photonVision.getLatestResult().getBestTarget().getPitch()); + * Shuffleboard.getTab("Vision").add("Limelight TX Value", + * LimelightHelpers.getTX("limelight")); + * Shuffleboard.getTab("Vision").add("Limelight April Tag ID", + * LimelightHelpers.getFiducialID("limelight")); + * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null); + * Shuffleboard.getTab("Vision").addCamera("Photon", + * "Arducam_OV9281_USB_Camera", + * "http://photonvision.local:5800"); + */ + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index c5abbbe..d677507 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -62,7 +62,7 @@ public class SwerveSubsystem extends SubsystemBase { /** * Enable vision odometry updates while driving. */ - private final boolean visionDriveTest = false; + private final boolean visionDriveTest = true; /** * PhotonVision class to keep an accurate odometry. diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 713fbb1..f8bc468 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -40,59 +40,57 @@ import org.photonvision.targeting.PhotonTrackedTarget; import swervelib.SwerveDrive; import swervelib.telemetry.SwerveDriveTelemetry; - /** - * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from + * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken + * from * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads */ -public class Vision -{ +public class Vision { /** * April Tag Field Layout of the year. */ - public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( + public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( AprilTagFields.k2026RebuiltAndymark); /** - * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. + * Ambiguity defined as a value between (0,1). Used in + * {@link Vision#filterPose}. */ - private final double maximumAmbiguity = 0.25; + private final double maximumAmbiguity = 0.25; /** * Photon Vision Simulation */ - public VisionSystemSim visionSim; + public VisionSystemSim visionSim; /** - * Count of times that the odom thinks we're more than 10meters away from the april tag. + * Count of times that the odom thinks we're more than 10meters away from the + * april tag. */ - private double longDistangePoseEstimationCount = 0; + private double longDistangePoseEstimationCount = 0; /** * Current pose from the pose estimator using wheel odometry. */ - private Supplier currentPose; + private Supplier currentPose; /** * Field from {@link swervelib.SwerveDrive#field} */ - private Field2d field2d; - + private Field2d field2d; /** * Constructor for the Vision class. * - * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} + * @param currentPose Current pose supplier, should reference + * {@link SwerveDrive#getPose()} * @param field Current field, should be {@link SwerveDrive#field} */ - public Vision(Supplier currentPose, Field2d field) - { + public Vision(Supplier currentPose, Field2d field) { this.currentPose = currentPose; this.field2d = field; - if (Robot.isSimulation()) - { + if (Robot.isSimulation()) { visionSim = new VisionSystemSim("Vision"); visionSim.addAprilTags(fieldLayout); - for (Cameras c : Cameras.values()) - { + for (Cameras c : Cameras.values()) { c.addToVisionSim(visionSim); } @@ -104,50 +102,48 @@ public class Vision * Calculates a target pose relative to an AprilTag on the field. * * @param aprilTag The ID of the AprilTag. - * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position + * @param robotOffset The offset {@link Transform2d} of the robot to apply to + * the pose for the robot to position * itself correctly. * @return The target pose of the AprilTag. */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) - { + public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); - if (aprilTagPose3d.isPresent()) - { + if (aprilTagPose3d.isPresent()) { return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); - } else - { + } else { throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); } } /** - * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. + * Update the pose estimation inside of {@link SwerveDrive} with all of the + * given poses. * * @param swerveDrive {@link SwerveDrive} instance. */ - public void updatePoseEstimation(SwerveDrive swerveDrive) - { - if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) - { + public void updatePoseEstimation(SwerveDrive swerveDrive) { + if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) { /* - * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting. + * In the maple-sim, odometry is simulated using encoder values, accounting for + * factors like skidding and drifting. * As a result, the odometry may not always be 100% accurate. - * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect. + * However, the vision system should be able to provide a reasonably accurate + * pose estimation, even when odometry is incorrect. * (This is why teams implement vision system to correct odometry.) - * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation. + * Therefore, we must ensure that the actual robot pose is provided in the + * simulator when updating the vision simulation during the simulation. */ visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); } - for (Cameras camera : Cameras.values()) - { + for (Cameras camera : Cameras.values()) { Optional poseEst = getEstimatedGlobalPose(camera); - if (poseEst.isPresent()) - { + if (poseEst.isPresent()) { var pose = poseEst.get(); swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), - pose.timestampSeconds, - camera.curStdDevs); + pose.timestampSeconds, + camera.curStdDevs); } } @@ -156,24 +152,22 @@ public class Vision /** * Generates the estimated robot pose. Returns empty if: *
    - *
  • No Pose Estimates could be generated
    • - *
  • The generated pose estimate was considered not accurate
    • + *
  • No Pose Estimates could be generated
    • + *
  • The generated pose estimate was considered not accurate
    • *
* - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate + * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and + * targets used to create the estimate */ - public Optional getEstimatedGlobalPose(Cameras camera) - { + public Optional getEstimatedGlobalPose(Cameras camera) { Optional poseEst = camera.getEstimatedGlobalPose(); - if (Robot.isSimulation()) - { + if (Robot.isSimulation()) { Field2d debugField = visionSim.getDebugField(); // Uncomment to enable outputting of vision targets in sim. poseEst.ifPresentOrElse( - est -> - debugField - .getObject("VisionEstimation") - .setPose(est.estimatedPose.toPose2d()), + est -> debugField + .getObject("VisionEstimation") + .setPose(est.estimatedPose.toPose2d()), () -> { debugField.getObject("VisionEstimation").setPoses(); }); @@ -181,46 +175,39 @@ public class Vision return poseEst; } - /** - * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than + * Filter pose via the ambiguity and find best estimate between all of the + * camera's throwing out distances more than * 10m for a short amount of time. * * @param pose Estimated robot pose. * @return Could be empty if there isn't a good reading. */ @Deprecated(since = "2024", forRemoval = true) - private Optional filterPose(Optional pose) - { - if (pose.isPresent()) - { + private Optional filterPose(Optional pose) { + if (pose.isPresent()) { double bestTargetAmbiguity = 1; // 1 is max ambiguity - for (PhotonTrackedTarget target : pose.get().targetsUsed) - { + for (PhotonTrackedTarget target : pose.get().targetsUsed) { double ambiguity = target.getPoseAmbiguity(); - if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) - { + if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) { bestTargetAmbiguity = ambiguity; } } - //ambiguity to high dont use estimate - if (bestTargetAmbiguity > maximumAmbiguity) - { + // ambiguity to high dont use estimate + if (bestTargetAmbiguity > maximumAmbiguity) { return Optional.empty(); } - //est pose is very far from recorded robot pose - if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) - { + // est pose is very far from recorded robot pose + if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) { longDistangePoseEstimationCount++; - //if it calculates that were 10 meter away for more than 10 times in a row its probably right - if (longDistangePoseEstimationCount < 10) - { + // if it calculates that were 10 meter away for more than 10 times in a row its + // probably right + if (longDistangePoseEstimationCount < 10) { return Optional.empty(); } - } else - { + } else { longDistangePoseEstimationCount = 0; } return pose; @@ -228,15 +215,13 @@ public class Vision return Optional.empty(); } - /** * Get distance of the robot from the AprilTag pose. * * @param id AprilTag ID * @return Distance */ - public double getDistanceFromAprilTag(int id) - { + public double getDistanceFromAprilTag(int id) { Optional tag = fieldLayout.getTagPose(id); return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); } @@ -248,17 +233,12 @@ public class Vision * @param camera Camera to check. * @return Tracked target. */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) - { + public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { PhotonTrackedTarget target = null; - for (PhotonPipelineResult result : camera.resultsList) - { - if (result.hasTargets()) - { - for (PhotonTrackedTarget i : result.getTargets()) - { - if (i.getFiducialId() == id) - { + for (PhotonPipelineResult result : camera.resultsList) { + if (result.hasTargets()) { + for (PhotonTrackedTarget i : result.getTargets()) { + if (i.getFiducialId() == id) { return i; } } @@ -273,54 +253,46 @@ public class Vision * * @return Vision Simulation */ - public VisionSystemSim getVisionSim() - { + public VisionSystemSim getVisionSim() { return visionSim; } /** - * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. + * Open up the photon vision camera streams on the localhost, assumes running + * photon vision on localhost. */ - private void openSimCameraViews() - { - if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) - { -// try -// { -// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); -// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); -// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); -// } catch (IOException | URISyntaxException e) -// { -// e.printStackTrace(); -// } + private void openSimCameraViews() { + if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) { + // try + // { + // Desktop.getDesktop().browse(new URI("http://localhost:1182/")); + // Desktop.getDesktop().browse(new URI("http://localhost:1184/")); + // Desktop.getDesktop().browse(new URI("http://localhost:1186/")); + // } catch (IOException | URISyntaxException e) + // { + // e.printStackTrace(); + // } } } /** * Update the {@link Field2d} to include tracked targets/ */ - public void updateVisionField() - { + public void updateVisionField() { List targets = new ArrayList(); - for (Cameras c : Cameras.values()) - { - if (!c.resultsList.isEmpty()) - { + for (Cameras c : Cameras.values()) { + if (!c.resultsList.isEmpty()) { PhotonPipelineResult latest = c.resultsList.get(0); - if (latest.hasTargets()) - { + if (latest.hasTargets()) { targets.addAll(latest.targets); } } } List poses = new ArrayList<>(); - for (PhotonTrackedTarget target : targets) - { - if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) - { + for (PhotonTrackedTarget target : targets) { + if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) { Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); poses.add(targetPose); } @@ -332,95 +304,100 @@ public class Vision /** * Camera Enum to select each camera */ - enum Cameras - { + enum Cameras { /** * Left Camera */ LEFT_CAM("left", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), + new Translation3d(Units.inchesToMeters(12.056), + Units.inchesToMeters(10.981), + Units.inchesToMeters(8.44)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), /** * Right Camera */ RIGHT_CAM("right", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(-10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), + new Translation3d(Units.inchesToMeters(12.056), + Units.inchesToMeters(-10.981), + Units.inchesToMeters(8.44)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), /** * Center Camera */ CENTER_CAM("center", - new Rotation3d(0, Units.degreesToRadians(18), 0), - new Translation3d(Units.inchesToMeters(-4.628), - Units.inchesToMeters(-10.687), - Units.inchesToMeters(16.129)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); + new Rotation3d(0, Units.degreesToRadians(18), 0), + new Translation3d(Units.inchesToMeters(-4.628), + Units.inchesToMeters(-10.687), + Units.inchesToMeters(16.129)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); /** * Latency alert to use when high latency is detected. */ - public final Alert latencyAlert; + public final Alert latencyAlert; /** * Camera instance for comms. */ - public final PhotonCamera camera; + public final PhotonCamera camera; /** * Pose estimator for camera. */ - public final PhotonPoseEstimator poseEstimator; + public final PhotonPoseEstimator poseEstimator; /** * Standard Deviation for single tag readings for pose estimation. */ - private final Matrix singleTagStdDevs; + private final Matrix singleTagStdDevs; /** * Standard deviation for multi-tag readings for pose estimation. */ - private final Matrix multiTagStdDevs; + private final Matrix multiTagStdDevs; /** - * Transform of the camera rotation and translation relative to the center of the robot + * Transform of the camera rotation and translation relative to the center of + * the robot */ - private final Transform3d robotToCamTransform; + private final Transform3d robotToCamTransform; /** * Current standard deviations used. */ - public Matrix curStdDevs; + public Matrix curStdDevs; /** * Estimated robot pose. */ - public Optional estimatedRobotPose = Optional.empty(); + public Optional estimatedRobotPose = Optional.empty(); /** * Simulated camera instance which only exists during simulations. */ - public PhotonCameraSim cameraSim; + public PhotonCameraSim cameraSim; /** - * Results list to be updated periodically and cached to avoid unnecessary queries. + * Results list to be updated periodically and cached to avoid unnecessary + * queries. */ - public List resultsList = new ArrayList<>(); + public List resultsList = new ArrayList<>(); /** * Last read from the camera timestamp to prevent lag due to slow data fetches. */ - private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); + private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); /** - * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine + * Construct a Photon Camera class with help. Standard deviations are fake + * values, experiment and determine * estimation noise on an actual robot. * - * @param name Name of the PhotonVision camera found in the PV UI. + * @param name Name of the PhotonVision camera found in the PV + * UI. * @param robotToCamRotation {@link Rotation3d} of the camera. - * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. - * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. - * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. + * @param robotToCamTranslation {@link Translation3d} relative to the center of + * the robot. + * @param singleTagStdDevs Single AprilTag standard deviations of estimated + * poses from the camera. + * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated + * poses from the camera. */ Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, - Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) - { + Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) { latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning); camera = new PhotonCamera(name); @@ -429,21 +406,22 @@ public class Vision robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamTransform); + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + robotToCamTransform); poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); this.singleTagStdDevs = singleTagStdDevs; this.multiTagStdDevs = multiTagStdDevsMatrix; - if (Robot.isSimulation()) - { + if (Robot.isSimulation()) { SimCameraProperties cameraProp = new SimCameraProperties(); // A 640 x 480 camera with a 100 degree diagonal FOV. cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in pixels. + // Approximate detection noise with average and standard deviation error in + // pixels. cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop rate). + // Set the camera image capture framerate (Note: this is limited by robot loop + // rate). cameraProp.setFPS(30); // The average and standard deviation in milliseconds of image data latency. cameraProp.setAvgLatencyMs(35); @@ -459,35 +437,31 @@ public class Vision * * @param systemSim {@link VisionSystemSim} to use. */ - public void addToVisionSim(VisionSystemSim systemSim) - { - if (Robot.isSimulation()) - { + public void addToVisionSim(VisionSystemSim systemSim) { + if (Robot.isSimulation()) { systemSim.addCamera(cameraSim, robotToCamTransform); } } /** - * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most + * Get the result with the least ambiguity from the best tracked target within + * the Cache. This may not be the most * recent result! * - * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result! + * @return The result in the cache with the least ambiguous best tracked target. + * This is not the most recent result! */ - public Optional getBestResult() - { - if (resultsList.isEmpty()) - { + public Optional getBestResult() { + if (resultsList.isEmpty()) { return Optional.empty(); } - PhotonPipelineResult bestResult = resultsList.get(0); - double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); - double currentAmbiguity = 0; - for (PhotonPipelineResult result : resultsList) - { + PhotonPipelineResult bestResult = resultsList.get(0); + double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); + double currentAmbiguity = 0; + for (PhotonPipelineResult result : resultsList) { currentAmbiguity = result.getBestTarget().getPoseAmbiguity(); - if (currentAmbiguity < amiguity && currentAmbiguity > 0) - { + if (currentAmbiguity < amiguity && currentAmbiguity > 0) { bestResult = result; amiguity = currentAmbiguity; } @@ -498,63 +472,63 @@ public class Vision /** * Get the latest result from the current cache. * - * @return Empty optional if nothing is found. Latest result if something is there. + * @return Empty optional if nothing is found. Latest result if something is + * there. */ - public Optional getLatestResult() - { + public Optional getLatestResult() { return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0)); } /** - * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the + * Get the estimated robot pose. Updates the current robot pose estimation, + * standard deviations, and flushes the * cache of results. * * @return Estimated pose. */ - public Optional getEstimatedGlobalPose() - { + public Optional getEstimatedGlobalPose() { updateUnreadResults(); return estimatedRobotPose; } /** - * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp. + * Update the latest results, cached with a maximum refresh rate of 1req/15ms. + * Sorts the list by timestamp. */ - private void updateUnreadResults() - { + private void updateUnreadResults() { double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds(); - - for (PhotonPipelineResult result : resultsList) - { + + for (PhotonPipelineResult result : resultsList) { mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds()); } - resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults(); - resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { - return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; - }); - if (!resultsList.isEmpty()) - { - updateEstimatedGlobalPose(); - } + resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults(); + resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { + return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; + }); + if (!resultsList.isEmpty()) { + updateEstimatedGlobalPose(); + } } /** - * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once + * The latest estimated robot pose on the field from vision data. This may be + * empty. This should only be called once * per loop. * - *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + *

+ * Also includes updates for the standard deviations, which can (optionally) be + * retrieved with * {@link Cameras#updateEstimationStdDevs} * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for - * estimation. + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate + * timestamp, and targets used for + * estimation. */ - private void updateEstimatedGlobalPose() - { + private void updateEstimatedGlobalPose() { Optional visionEst = Optional.empty(); - for (var change : resultsList) - { + for (var change : resultsList) { visionEst = poseEstimator.update(change); updateEstimationStdDevs(visionEst, change.getTargets()); } @@ -562,63 +536,54 @@ public class Vision } /** - * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based + * Calculates new standard deviations This algorithm is a heuristic that creates + * dynamic standard deviations based * on number of tags, estimation strategy, and distance from the tags. * * @param estimatedPose The estimated pose to guess standard deviations for. * @param targets All targets in this camera frame */ private void updateEstimationStdDevs( - Optional estimatedPose, List targets) - { - if (estimatedPose.isEmpty()) - { + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { // No pose input. Default to single-tag std devs curStdDevs = singleTagStdDevs; - } else - { + } else { // Pose present. Start running Heuristic - var estStdDevs = singleTagStdDevs; - int numTags = 0; - double avgDist = 0; + var estStdDevs = singleTagStdDevs; + int numTags = 0; + double avgDist = 0; - // Precalculation - see how many tags we found, and calculate an average-distance metric - for (var tgt : targets) - { + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) - { + if (tagPose.isEmpty()) { continue; } numTags++; - avgDist += - tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); } - if (numTags == 0) - { + if (numTags == 0) { // No tags visible. Default to single-tag std devs curStdDevs = singleTagStdDevs; - } else - { + } else { // One or more tags visible, run the full heuristic. avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) - { + if (numTags > 1) { estStdDevs = multiTagStdDevs; } // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - { + if (numTags == 1 && avgDist > 4) { estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - } else - { + } else { estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); } curStdDevs = estStdDevs;