From 6c509f9ca806cc2257e0b2d9200d3796e1a6ce6a Mon Sep 17 00:00:00 2001 From: Pix_XL Date: Fri, 28 Mar 2025 00:37:10 -0400 Subject: [PATCH] my 13th reason --- src/main/java/frc/robot/RobotContainer.java | 37 +- .../java/frc/robot/driver/DriverXbox.java | 14 +- .../drivetrain/CommandSwerveDrivetrain.java | 2 +- .../commands/DriveAndHoldAngle.java | 2 +- .../commands/DriveToPoseCommand.java | 296 +++++----- .../java/frc/robot/vision/CamLocalizer.java | 36 ++ .../vision/LoggedPhotonVisionLocalizer.java | 153 +++++ .../java/frc/robot/vision/PhotonRunnable.java | 83 --- .../robot/vision/PhotonVisionLocalizer.java | 120 ++++ .../robot/vision/PoseEstimatorSubsystem.java | 254 -------- .../java/frc/robot/vision/TriConsumer.java | 19 + .../java/frc/robot/vision/VisionConfig.java | 372 +++++++----- .../vision/VisionLocalizationSystem.java | 113 ++++ .../frc/robot/vision/commands/AutoAlign.java | 550 +++++++++--------- .../commands/DirectDriveToPoseCommand.java | 83 +++ .../robot/vision/commands/LineupCommand.java | 492 ++++++++-------- .../robot/vision/commands/VisionCommands.java | 34 -- .../vision/old vision/PhotonRunnable.java | 83 +++ .../old vision/PoseEstimatorSubsystem.java | 254 ++++++++ 19 files changed, 1801 insertions(+), 1196 deletions(-) create mode 100644 src/main/java/frc/robot/vision/CamLocalizer.java create mode 100644 src/main/java/frc/robot/vision/LoggedPhotonVisionLocalizer.java delete mode 100644 src/main/java/frc/robot/vision/PhotonRunnable.java create mode 100644 src/main/java/frc/robot/vision/PhotonVisionLocalizer.java delete mode 100644 src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java create mode 100644 src/main/java/frc/robot/vision/TriConsumer.java create mode 100644 src/main/java/frc/robot/vision/VisionLocalizationSystem.java create mode 100644 src/main/java/frc/robot/vision/commands/DirectDriveToPoseCommand.java delete mode 100644 src/main/java/frc/robot/vision/commands/VisionCommands.java create mode 100644 src/main/java/frc/robot/vision/old vision/PhotonRunnable.java create mode 100644 src/main/java/frc/robot/vision/old vision/PoseEstimatorSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9cb97f5..8f89fee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,15 +1,11 @@ package frc.robot; -import com.ctre.phoenix6.swerve.SwerveRequest; - import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 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.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.algaeflywheel.AlgaeRoller; import frc.robot.algaepivot.AlgaeSubsystem; import frc.robot.auton.AutonMaster; @@ -19,13 +15,22 @@ import frc.robot.elevator.ElevatorSubsystem; import static edu.wpi.first.units.Units.*; +import static frc.robot.vision.VisionConfig.fieldLayout; + +import org.photonvision.PhotonCamera; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + import frc.robot.operator.OperatorXbox; -import frc.robot.vision.PoseEstimatorSubsystem; -import frc.robot.vision.VisionConfig; +// import frc.robot.vision.PoseEstimatorSubsystem; +// import frc.robot.vision.VisionConfig; import frc.robot.rushinator.*; import frc.robot.rushinator.commands.*; +import frc.robot.vision.PhotonVisionLocalizer; +import frc.robot.vision.VisionConfig.PhotonConfig; +import frc.robot.vision.VisionLocalizationSystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -39,6 +44,8 @@ public class RobotContainer { public static boolean modeFast = true; + private final VisionLocalizationSystem m_vision = new VisionLocalizationSystem(); + /* Setting up bindings for necessary control of the swerve drive platform */ public static SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(kMaxVelocity * 0.1) @@ -56,10 +63,24 @@ public RobotContainer() { autonTab.add(mAutonChooser); SmartDashboard.putData(mAutonChooser); - var mPoseEstimator = PoseEstimatorSubsystem.getInstance(); + // var mPoseEstimator = PoseEstimatorSubsystem.getInstance(); // ShuffleboardTab visionTab = Shuffleboard.getTab("Vision Pose Estimator"); // PoseEstimatorSubsystem.getInstance().addDashboardWidgets(visionTab); + + for (PhotonConfig config : PhotonConfig.values()){ + + var cam = new PhotonCamera(config.name); + m_vision.addCamera(new PhotonVisionLocalizer( + cam, + config.offset, + config.multiTagPoseStrategy, + config.singleTagPoseStrategy, + () -> CommandSwerveDrivetrain.getInstance().getPose().getRotation(), + fieldLayout, + config.defaultSingleTagStdDevs, + config.defaultMultiTagStdDevs)); + } } /** @@ -144,5 +165,5 @@ public void setDefaultCommands() { // CoralSubsystem.getInstance().setDefaultCommand(new CoralSubsystem.TuningCommand(() -> (driver.getRightX() + 1) / 2.0f)); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java index 3bf148b..f809494 100644 --- a/src/main/java/frc/robot/driver/DriverXbox.java +++ b/src/main/java/frc/robot/driver/DriverXbox.java @@ -1,7 +1,7 @@ package frc.robot.driver; -import static frc.robot.vision.VisionConfig.AlignmentConfig.Cblue; -import static frc.robot.vision.VisionConfig.AlignmentConfig.Dblue; +// import static frc.robot.vision.VisionConfig.AlignmentConfig.Cblue; +// import static frc.robot.vision.VisionConfig.AlignmentConfig.Dblue; import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Translation2d; @@ -22,7 +22,7 @@ import frc.robot.climber.commands.SetClimberAngle; import frc.robot.commands.RobotCommands; import frc.robot.drivetrain.CommandSwerveDrivetrain; -import frc.robot.drivetrain.commands.DriveToPoseCommand; +// import frc.robot.drivetrain.commands.DriveToPoseCommand; import frc.robot.elevator.ElevatorSubsystem; import frc.robot.elevator.commands.ElevatorCommands; import frc.robot.elevator.commands.SetElevatorState; @@ -34,10 +34,10 @@ import frc.robot.rushinator.commands.SetWristState; import frc.robot.rushinator.commands.ToggleWristState; import frc.robot.rushinator.commands.SetRollersVoltage; -import frc.robot.vision.PoseEstimatorSubsystem; -import frc.robot.vision.VisionConfig; -import frc.robot.vision.commands.AutoAlign; -import frc.robot.vision.commands.LineupCommand; +// import frc.robot.vision.PoseEstimatorSubsystem; +// import frc.robot.vision.VisionConfig; +// import frc.robot.vision.commands.AutoAlign; +// import frc.robot.vision.commands.LineupCommand; public class DriverXbox extends XboxGamepad { diff --git a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java index 8c5cdaa..c6adec9 100644 --- a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java @@ -35,7 +35,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotContainer; import frc.robot.drivetrain.TunerConstants.TunerSwerveDrivetrain; -import frc.robot.vision.PoseEstimatorSubsystem; +// import frc.robot.vision.PoseEstimatorSubsystem; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveAndHoldAngle.java b/src/main/java/frc/robot/drivetrain/commands/DriveAndHoldAngle.java index 5a9f231..f3d2334 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveAndHoldAngle.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveAndHoldAngle.java @@ -17,7 +17,7 @@ import frc.robot.RobotContainer; import frc.robot.driver.DriverXbox; import frc.robot.drivetrain.CommandSwerveDrivetrain; -import frc.robot.vision.PoseEstimatorSubsystem; +// import frc.robot.vision.PoseEstimatorSubsystem; public class DriveAndHoldAngle extends Command{ private static class Settings { diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java index 08d2f37..dd7e473 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java @@ -1,148 +1,148 @@ -package frc.robot.drivetrain.commands; - - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static frc.robot.vision.VisionConfig.AlignmentConfig.*; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; -import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.drivetrain.CommandSwerveDrivetrain; - -import java.util.function.Supplier; - -/** - * Command to drive to a pose. - */ -public class DriveToPoseCommand extends Command { - - private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); - private static final Angle THETA_TOLERANCE = Degrees.of(1.0); - - protected static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = new TrapezoidProfile.Constraints( - MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), - MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); - protected static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints( - MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), - MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); - - private final ProfiledPIDController xController; - private final ProfiledPIDController yController; - private final ProfiledPIDController thetaController; - - private final CommandSwerveDrivetrain drivetrainSubsystem; - private final FieldCentric fieldCentricSwerveRequest = new FieldCentric() - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDriveRequestType(DriveRequestType.Velocity) - .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); // Always Blue coordinate system for auto drive - protected final Supplier poseProvider; - - /** - * Constructs a DriveToPoseCommand - * - * @param drivetrainSubsystem drivetrain subsystem - * @param goalPose goal pose to drive to - */ - public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider, Pose2d goalPose) { - this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); - thetaController.setGoal(goalPose.getRotation().getRadians()); - xController.setGoal(goalPose.getX()); - yController.setGoal(goalPose.getY()); - } - - /** - * Constructs a DriveToPoseCommand with specific motion profile constraints - * - * @param drivetrainSubsystem drivetrain subsystem - * @param poseProvider provider to call to get the robot pose - * @param translationConstraints translation motion profile constraints - * @param omegaConstraints rotation motion profile constraints - */ - public DriveToPoseCommand( - CommandSwerveDrivetrain drivetrainSubsystem, - Supplier poseProvider, - TrapezoidProfile.Constraints translationConstraints, - TrapezoidProfile.Constraints omegaConstraints) { - - this.drivetrainSubsystem = drivetrainSubsystem; - this.poseProvider = poseProvider; - - xController = new ProfiledPIDController(X_kP, X_kI, X_kD, translationConstraints); - xController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); - - yController = new ProfiledPIDController(Y_kP, Y_kI, Y_kD, translationConstraints); - yController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); - - thetaController = new ProfiledPIDController(THETA_kP, THETA_kI, THETA_kD, omegaConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - thetaController.setTolerance(THETA_TOLERANCE.in(Radians)); - - addRequirements(drivetrainSubsystem); - } - - /** - * Sets the goal to drive to. This should be set before the command is scheduled. - * - * @param goalPose goal pose - */ - public void setGoal(Pose2d goalPose) { - thetaController.setGoal(goalPose.getRotation().getRadians()); - xController.setGoal(goalPose.getX()); - yController.setGoal(goalPose.getY()); - } - - @Override - public void initialize() { - var robotPose = poseProvider.get(); - thetaController.reset(robotPose.getRotation().getRadians()); - xController.reset(robotPose.getX()); - yController.reset(robotPose.getY()); - } - - @Override - public void execute() { - var robotPose = poseProvider.get(); - - var xSpeed = xController.calculate(robotPose.getX()); - if (xController.atGoal()) { - xSpeed = 0; - } - - var ySpeed = yController.calculate(robotPose.getY()); - if (yController.atGoal()) { - ySpeed = 0; - } - - var omegaSpeed = thetaController.calculate(robotPose.getRotation().getRadians()); - if (thetaController.atGoal()) { - omegaSpeed = 0; - } - - drivetrainSubsystem.setControl( - fieldCentricSwerveRequest.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(omegaSpeed)); - } - - @Override - public boolean isFinished() { - return xController.atGoal() && yController.atGoal() && thetaController.atGoal(); - } - - @Override - public void end(boolean interrupted) { - drivetrainSubsystem.setControl(new SwerveRequest.Idle()); - } -} \ No newline at end of file +// package frc.robot.drivetrain.commands; + + +// import static edu.wpi.first.units.Units.Degrees; +// import static edu.wpi.first.units.Units.Inches; +// import static edu.wpi.first.units.Units.Meters; +// import static edu.wpi.first.units.Units.MetersPerSecond; +// import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +// import static edu.wpi.first.units.Units.Radians; +// import static edu.wpi.first.units.Units.RadiansPerSecond; +// import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +// // import static frc.robot.vision.VisionConfig.AlignmentConfig.*; +// import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +// import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +// import com.ctre.phoenix6.swerve.SwerveRequest; +// import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; +// import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.trajectory.TrapezoidProfile; +// import edu.wpi.first.units.measure.Angle; +// import edu.wpi.first.units.measure.Distance; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.drivetrain.CommandSwerveDrivetrain; + +// import java.util.function.Supplier; + +// /** +// * Command to drive to a pose. +// */ +// public class DriveToPoseCommand extends Command { + +// private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); +// private static final Angle THETA_TOLERANCE = Degrees.of(1.0); + +// protected static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = new TrapezoidProfile.Constraints( +// MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), +// MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); +// protected static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints( +// MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), +// MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); + +// private final ProfiledPIDController xController; +// private final ProfiledPIDController yController; +// private final ProfiledPIDController thetaController; + +// private final CommandSwerveDrivetrain drivetrainSubsystem; +// private final FieldCentric fieldCentricSwerveRequest = new FieldCentric() +// .withSteerRequestType(SteerRequestType.MotionMagicExpo) +// .withDriveRequestType(DriveRequestType.Velocity) +// .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); // Always Blue coordinate system for auto drive +// protected final Supplier poseProvider; + +// /** +// * Constructs a DriveToPoseCommand +// * +// * @param drivetrainSubsystem drivetrain subsystem +// * @param goalPose goal pose to drive to +// */ +// public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider, Pose2d goalPose) { +// this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); +// thetaController.setGoal(goalPose.getRotation().getRadians()); +// xController.setGoal(goalPose.getX()); +// yController.setGoal(goalPose.getY()); +// } + +// /** +// * Constructs a DriveToPoseCommand with specific motion profile constraints +// * +// * @param drivetrainSubsystem drivetrain subsystem +// * @param poseProvider provider to call to get the robot pose +// * @param translationConstraints translation motion profile constraints +// * @param omegaConstraints rotation motion profile constraints +// */ +// public DriveToPoseCommand( +// CommandSwerveDrivetrain drivetrainSubsystem, +// Supplier poseProvider, +// TrapezoidProfile.Constraints translationConstraints, +// TrapezoidProfile.Constraints omegaConstraints) { + +// this.drivetrainSubsystem = drivetrainSubsystem; +// this.poseProvider = poseProvider; + +// xController = new ProfiledPIDController(X_kP, X_kI, X_kD, translationConstraints); +// xController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + +// yController = new ProfiledPIDController(Y_kP, Y_kI, Y_kD, translationConstraints); +// yController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + +// thetaController = new ProfiledPIDController(THETA_kP, THETA_kI, THETA_kD, omegaConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); +// thetaController.setTolerance(THETA_TOLERANCE.in(Radians)); + +// addRequirements(drivetrainSubsystem); +// } + +// /** +// * Sets the goal to drive to. This should be set before the command is scheduled. +// * +// * @param goalPose goal pose +// */ +// public void setGoal(Pose2d goalPose) { +// thetaController.setGoal(goalPose.getRotation().getRadians()); +// xController.setGoal(goalPose.getX()); +// yController.setGoal(goalPose.getY()); +// } + +// @Override +// public void initialize() { +// var robotPose = poseProvider.get(); +// thetaController.reset(robotPose.getRotation().getRadians()); +// xController.reset(robotPose.getX()); +// yController.reset(robotPose.getY()); +// } + +// @Override +// public void execute() { +// var robotPose = poseProvider.get(); + +// var xSpeed = xController.calculate(robotPose.getX()); +// if (xController.atGoal()) { +// xSpeed = 0; +// } + +// var ySpeed = yController.calculate(robotPose.getY()); +// if (yController.atGoal()) { +// ySpeed = 0; +// } + +// var omegaSpeed = thetaController.calculate(robotPose.getRotation().getRadians()); +// if (thetaController.atGoal()) { +// omegaSpeed = 0; +// } + +// drivetrainSubsystem.setControl( +// fieldCentricSwerveRequest.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(omegaSpeed)); +// } + +// @Override +// public boolean isFinished() { +// return xController.atGoal() && yController.atGoal() && thetaController.atGoal(); +// } + +// @Override +// public void end(boolean interrupted) { +// drivetrainSubsystem.setControl(new SwerveRequest.Idle()); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/CamLocalizer.java b/src/main/java/frc/robot/vision/CamLocalizer.java new file mode 100644 index 0000000..942375b --- /dev/null +++ b/src/main/java/frc/robot/vision/CamLocalizer.java @@ -0,0 +1,36 @@ +// Copyright (c) FRC 1076 PiHi Samurai +// You may use, distribute, and modify this software under the terms of +// the license found in the root directory of this project + +package frc.robot.vision; + +import java.util.Optional; +import java.util.function.Supplier; + +import org.photonvision.PhotonPoseEstimator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** A generic interface for handling all camera objects */ +public interface CamLocalizer { + + public static record CommonPoseEstimate( + Pose2d pose, + double timestampSeconds, + Matrix stdDevs + ) {} + + public abstract Optional getPoseEstimate(); + + public abstract String getName(); + + public default void addHeadingSupplier(Supplier heading) {} + + public default void setPoseStrategy(PhotonPoseEstimator.PoseStrategy strategy) {} + + public default void setFallbackPoseStrategy(PhotonPoseEstimator.PoseStrategy strategy) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/LoggedPhotonVisionLocalizer.java b/src/main/java/frc/robot/vision/LoggedPhotonVisionLocalizer.java new file mode 100644 index 0000000..4f62b11 --- /dev/null +++ b/src/main/java/frc/robot/vision/LoggedPhotonVisionLocalizer.java @@ -0,0 +1,153 @@ +package frc.robot.vision; + +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; + +public class LoggedPhotonVisionLocalizer implements CamLocalizer { + + public static class PhotonVisionInputs { + public boolean cameraConnected = false; + public boolean estimatePresent = false; + public int tagsDetected = 0; + public Integer[] fiducialIDs = new Integer[]{}; + public Pose3d pose = new Pose3d(); + public Matrix stddevs = VecBuilder.fill(0, 0, 0); + public String strategy = "NULL"; + + public void log(String key) { + Logger.recordOutput(key, cameraConnected); + Logger.recordOutput(key, estimatePresent); + Logger.recordOutput(key, tagsDetected); + Logger.recordOutput(key, fiducialIDs.toString()); + Logger.recordOutput(key, pose); + Logger.recordOutput(key, stddevs); + Logger.recordOutput(key, strategy); + } + } + + + private static final Matrix maxStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + private final PhotonCamera camera; + private final PhotonPoseEstimator poseEstimator; + private final Supplier headingSupplier; + private final Matrix defaultSingleStdDevs; + private final Matrix defaultMultiStdDevs; + private final PhotonVisionInputs inputs = new PhotonVisionInputs(); + + public LoggedPhotonVisionLocalizer( + PhotonCamera camera, + Transform3d offset, + PhotonPoseEstimator.PoseStrategy primaryStrategy, + PhotonPoseEstimator.PoseStrategy multiTagFallbackStrategy, + Supplier headingSupplier, + AprilTagFieldLayout fieldLayout, + Matrix defaultSingleStdDevs, + Matrix defaultMultiStdDevs + ) { + this.camera = camera; + this.poseEstimator = new PhotonPoseEstimator(fieldLayout, primaryStrategy, offset); + poseEstimator.setMultiTagFallbackStrategy(multiTagFallbackStrategy); + this.headingSupplier = headingSupplier; + this.defaultSingleStdDevs = defaultSingleStdDevs; + this.defaultMultiStdDevs = defaultMultiStdDevs; + } + + /** + * Calculates the standard deviations for the pose estimate based on how many tags are visible and how far they are + */ + private Matrix calculateStdDevs(EstimatedRobotPose est) { + var stdDevs = defaultSingleStdDevs; + int numTargets = 0; + double avgDist = 0; + var targets = est.targetsUsed; + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) {continue;} + numTargets++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(est.estimatedPose.toPose2d().getTranslation()); + } + if (numTargets == 0) { + return maxStdDevs; //No targets detected, resort to maximum std devs + } + + // One or more tags visible, run the full heuristic. + + // Decrease std devs if multiple targets are visible + avgDist /= numTargets; + if (numTargets > 1) { + stdDevs = defaultMultiStdDevs; + } + + // Increase std devs based on (average) distance + if (numTargets == 1 && avgDist > 4){ + //Distance greater than 4 meters, and only one tag detected, resort to maximum std devs + stdDevs = maxStdDevs; + } else { + stdDevs = stdDevs.times(1 + (avgDist * avgDist / 30)); + } + return stdDevs; + } + + /** + * Gets the pose estimate from the camera + * @return The pose estimate, or Optional.empty() if no estimate is available + */ + public Optional getPoseEstimate() { + + inputs.cameraConnected = camera.isConnected(); + inputs.estimatePresent = false; + + poseEstimator.addHeadingData(Timer.getFPGATimestamp(), headingSupplier.get()); + List results = camera.getAllUnreadResults(); + Optional visionEst = Optional.empty(); + + for (var res : results) { + visionEst = poseEstimator.update(res); + } + + Optional result = visionEst.map( + (EstimatedRobotPose estimate) -> { + var stddevs = calculateStdDevs(estimate); + inputs.tagsDetected = estimate.targetsUsed.size(); + inputs.fiducialIDs = estimate.targetsUsed.stream().map((tgt) -> tgt.getFiducialId()).toArray((size) -> new Integer[size]); + inputs.pose = estimate.estimatedPose; + inputs.stddevs = stddevs; + inputs.strategy = estimate.strategy.name(); + return new CommonPoseEstimate( + estimate.estimatedPose.toPose2d(), + estimate.timestampSeconds, + stddevs + ); + } + ); + + // inputs.log("PhotonVision/" + getName()); + + return result; + } + + public String getName() { + return camera.getName(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/PhotonRunnable.java b/src/main/java/frc/robot/vision/PhotonRunnable.java deleted file mode 100644 index d62ac90..0000000 --- a/src/main/java/frc/robot/vision/PhotonRunnable.java +++ /dev/null @@ -1,83 +0,0 @@ -package frc.robot.vision; - -import static frc.robot.vision.VisionConfig.AMBIGUITY_THRESHOLD; - -import java.io.IOException; -import java.util.concurrent.atomic.AtomicReference; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Runnable that gets AprilTag data from PhotonVision. - */ -public class PhotonRunnable implements Runnable { - - private final PhotonPoseEstimator photonPoseEstimator; - private final PhotonCamera photonCamera; - private final AtomicReference atomicEstimatedRobotPose = new AtomicReference(); - - public PhotonRunnable(PhotonCamera cameraName, Transform3d robotToCamera) { - this.photonCamera = cameraName; - PhotonPoseEstimator photonPoseEstimator = null; - try { - var layout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - // PV estimates will always be blue, they'll get flipped by robot thread - layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - if (photonCamera != null) { - photonPoseEstimator = new PhotonPoseEstimator( - layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); - photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - } - } catch (Exception e) { - DriverStation.reportError("Try Catch Block Fail in PhotonRunnable.java", e.getStackTrace()); - photonPoseEstimator = null; - } - this.photonPoseEstimator = photonPoseEstimator; - } - - @Override - public void run() { - // Get AprilTag data - if (photonPoseEstimator != null && photonCamera != null) { - var photonResults = photonCamera.getAllUnreadResults(); - for(var result : photonResults) { - if (result.hasTargets() - && (result.targets.size() > 1 - || result.targets.get(0).getPoseAmbiguity() < AMBIGUITY_THRESHOLD)) { - photonPoseEstimator.update(result).ifPresent(estimatedRobotPose -> { - var estimatedPose = estimatedRobotPose.estimatedPose; - // Make sure the measurement is on the field - if (estimatedPose.getX() > 0.0 && estimatedPose.getX() <= VisionConfig.FIELD_LENGTH_METERS - && estimatedPose.getY() > 0.0 && estimatedPose.getY() <= VisionConfig.FIELD_WIDTH_METERS) { - atomicEstimatedRobotPose.set(estimatedRobotPose); - } - }); - } - } - - } - } - - /** - * Gets the latest robot pose. Calling this will only return the pose once. If - * it returns a non-null value, it is a - * new estimate that hasn't been returned before. - * This pose will always be for the BLUE alliance. It must be flipped if the - * current alliance is RED. - * - * @return latest estimated pose - */ - public EstimatedRobotPose grabLatestEstimatedPose() { - return atomicEstimatedRobotPose.getAndSet(null); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/PhotonVisionLocalizer.java b/src/main/java/frc/robot/vision/PhotonVisionLocalizer.java new file mode 100644 index 0000000..79e02c4 --- /dev/null +++ b/src/main/java/frc/robot/vision/PhotonVisionLocalizer.java @@ -0,0 +1,120 @@ +// Copyright (c) FRC 1076 PiHi Samurai +// You may use, distribute, and modify this software under the terms of +// the license found in the root directory of this project + +package frc.robot.vision; + +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; + +public class PhotonVisionLocalizer implements CamLocalizer { + + private static final Matrix maxStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + private final PhotonCamera camera; + private final PhotonPoseEstimator poseEstimator; + private final Supplier headingSupplier; + private final Matrix defaultSingleStdDevs; + private final Matrix defaultMultiStdDevs; + + public PhotonVisionLocalizer( + PhotonCamera camera, + Transform3d offset, + PhotonPoseEstimator.PoseStrategy primaryStrategy, + PhotonPoseEstimator.PoseStrategy multiTagFallbackStrategy, + Supplier headingSupplier, + AprilTagFieldLayout fieldLayout, + Matrix defaultSingleStdDevs, + Matrix defaultMultiStdDevs + ) { + this.camera = camera; + this.poseEstimator = new PhotonPoseEstimator(fieldLayout, primaryStrategy, offset); + poseEstimator.setMultiTagFallbackStrategy(multiTagFallbackStrategy); + this.headingSupplier = headingSupplier; + this.defaultSingleStdDevs = defaultSingleStdDevs; + this.defaultMultiStdDevs = defaultMultiStdDevs; + } + + /** + * Calculates the standard deviations for the pose estimate based on how many tags are visible and how far they are + */ + private Matrix calculateStdDevs(EstimatedRobotPose est) { + var stdDevs = defaultSingleStdDevs; + int numTargets = 0; + double avgDist = 0; + var targets = est.targetsUsed; + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) {continue;} + numTargets++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(est.estimatedPose.toPose2d().getTranslation()); + } + if (numTargets == 0) { + return maxStdDevs; //No targets detected, resort to maximum std devs + } + + // One or more tags visible, run the full heuristic. + + // Decrease std devs if multiple targets are visible + avgDist /= numTargets; + if (numTargets > 1) { + stdDevs = defaultMultiStdDevs; + } + + // Increase std devs based on (average) distance + if (numTargets == 1 && avgDist > 4){ + //Distance greater than 4 meters, and only one tag detected, resort to maximum std devs + stdDevs = maxStdDevs; + } else { + stdDevs = stdDevs.times(1 + (avgDist * avgDist / 30)); + } + return stdDevs; + } + + /** + * Gets the pose estimate from the camera + * @return The pose estimate, or Optional.empty() if no estimate is available + */ + public Optional getPoseEstimate() { + poseEstimator.addHeadingData(Timer.getFPGATimestamp(), headingSupplier.get()); + List results = camera.getAllUnreadResults(); + Optional visionEst = Optional.empty(); + + for (var res : results) { + visionEst = poseEstimator.update(res); + } + + return visionEst.map( + (EstimatedRobotPose estimate) -> { + return new CommonPoseEstimate( + estimate.estimatedPose.toPose2d(), + estimate.timestampSeconds, + calculateStdDevs(estimate) + ); + } + ); + } + + public String getName() { + return camera.getName(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java b/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java deleted file mode 100644 index aae66e0..0000000 --- a/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java +++ /dev/null @@ -1,254 +0,0 @@ -package frc.robot.vision; - -import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide; -import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide; - -import java.util.function.Supplier; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; - -import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.drivetrain.CommandSwerveDrivetrain; - -public class PoseEstimatorSubsystem extends SubsystemBase { - - // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much - // you trust your various sensors. Smaller numbers will cause the filter to - // "trust" the estimate from that particular component more than the others. - // This in turn means the particualr component will have a stronger influence - // on the final pose estimate. - - // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much - // you trust your various sensors. Smaller numbers will cause the filter to - // "trust" the estimate from that particular component more than the others. - // This in turn means the particualr component will have a stronger influence - // on the final pose estimate. - - private final Supplier rotationSupplier; - private final Supplier modulePositionSupplier; - private final SwerveDrivePoseEstimator poseEstimator; - private final Field2d field2d = new Field2d(); - private final PhotonRunnable rightEstimator = new PhotonRunnable(new PhotonCamera(VisionConfig.CAM_NAMES[1]), - VisionConfig.ROBOT_TO_CAM_TRANSFORMS[1]); - private final PhotonRunnable leftEstimator = new PhotonRunnable(new PhotonCamera(VisionConfig.CAM_NAMES[0]), - VisionConfig.ROBOT_TO_CAM_TRANSFORMS[0]); - - private final Notifier allNotifier = new Notifier(() -> { - rightEstimator.run(); - leftEstimator.run(); - }); - - private OriginPosition originPosition = kBlueAllianceWallRightSide; - - private static PoseEstimatorSubsystem mInstance; - - public PoseEstimatorSubsystem(Supplier rotationSupplier, - Supplier modulePositionSupplier) { - this.rotationSupplier = rotationSupplier; - this.modulePositionSupplier = modulePositionSupplier; - - poseEstimator = new SwerveDrivePoseEstimator( - CommandSwerveDrivetrain.getInstance().getKinematics(), - rotationSupplier.get(), - modulePositionSupplier.get(), - new Pose2d(), - VisionConfig.STATE_STANDARD_DEVIATIONS, - VisionConfig.VISION_MEASUREMENT_STANDARD_DEVIATIONS); - - // Start PhotonVision thread - // rightNotifier.setName("rightRunnable"); - // rightNotifier.startPeriodic(0.02); - - // // Start PhotonVision thread - // leftNotifier.setName("leftRunnable"); - // leftNotifier.startPeriodic(0.02); - - allNotifier.setName("runAll"); - allNotifier.startPeriodic(0.02); - - SmartDashboard.putData("Field Pose Estimation", field2d); - // backNotifier.setName("backRunnable"); - // backNotifier.startPeriodic(0.02); - - } - - public static PoseEstimatorSubsystem getInstance() { - if(mInstance == null) { - CommandSwerveDrivetrain mDrivetrain = CommandSwerveDrivetrain.getInstance(); - mInstance = new PoseEstimatorSubsystem(mDrivetrain::getGyroRotation, mDrivetrain::getSwerveModulePositions); - } - return mInstance; - } - - public void addDashboardWidgets(ShuffleboardTab tab) { - tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); - tab.addString("Pose", this::getFomattedPose).withPosition(6, 2).withSize(2, 1); - } - - /** - * Sets the alliance. This is used to configure the origin of the AprilTag map - * - * @param alliance alliance - */ - public void setAlliance(Alliance alliance) { - boolean allianceChanged = false; - switch (alliance) { - case Blue: - allianceChanged = (originPosition == kRedAllianceWallRightSide); - originPosition = kBlueAllianceWallRightSide; - break; - case Red: - allianceChanged = (originPosition == kBlueAllianceWallRightSide); - originPosition = kRedAllianceWallRightSide; - break; - default: - // No valid alliance data. Nothing we can do about it - } - - if (allianceChanged) { - // The alliance changed, which changes the coordinate system. - // Since a tag was seen, and the tags are all relative to the coordinate system, - // the estimated pose - // needs to be transformed to the new coordinate system. - var newPose = flipAlliance(getCurrentPose()); - poseEstimator.resetPosition(rotationSupplier.get(), modulePositionSupplier.get(), newPose); - } - } - - @Override - public void periodic() { - // Update pose estimator with drivetrain sensors - poseEstimator.update(rotationSupplier.get(), modulePositionSupplier.get()); - if (VisionConfig.USE_VISION) { - estimatorChecker(rightEstimator); - estimatorChecker(leftEstimator); - } else { - allNotifier.close(); - } - - // estimatorChecker(backEstimator); - - // Set the pose on the dashboard - var dashboardPose = poseEstimator.getEstimatedPosition(); - if (originPosition == kRedAllianceWallRightSide) { - // Flip the pose when red, since the dashboard field photo cannot be rotated - dashboardPose = flipAlliance(dashboardPose); - } - field2d.setRobotPose(dashboardPose); - SmartDashboard.putString("Pose Formatted", getFomattedPose()); - - } - - private String getFomattedPose() { - var pose = getCurrentPose(); - return String.format("(%.3f, %.3f) %.2f degrees", - pose.getX(), - pose.getY(), - pose.getRotation().getDegrees()); - } - - public Pose2d getCurrentPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** - * Resets the current pose to the specified pose. This should ONLY be called - * when the robot's position on the field is known, like at the beginning of - * a match. - * - * @param newPose new pose - */ - public void setCurrentPose(Pose2d newPose) { - poseEstimator.resetPosition(rotationSupplier.get(), modulePositionSupplier.get(), newPose); - } - - /** - * Resets the position on the field to 0,0 0-degrees, with forward being - * downfield. This resets - * what "forward" is for field oriented driving. - */ - public void resetFieldPosition() { - setCurrentPose(new Pose2d()); - } - - /** - * Transforms a pose to the opposite alliance's coordinate system. (0,0) is - * always on the right corner of your - * alliance wall, so for 2023, the field elements are at different coordinates - * for each alliance. - * - * @param poseToFlip pose to transform to the other alliance - * @return pose relative to the other alliance's coordinate system - */ - private Pose2d flipAlliance(Pose2d poseToFlip) { - return poseToFlip.relativeTo(new Pose2d( - new Translation2d(VisionConfig.FIELD_LENGTH_METERS, VisionConfig.FIELD_WIDTH_METERS), - new Rotation2d(Math.PI))); - } - - public void addTrajectory(Trajectory traj) { - field2d.getObject("Trajectory").setTrajectory(traj); - } - - // public void resetPoseRating() { - // xValues.clear(); - // yValues.clear(); - // } - - private Matrix confidenceCalculator(EstimatedRobotPose estimation) { - double smallestDistance = Double.POSITIVE_INFINITY; - for (var target : estimation.targetsUsed) { - var t3d = target.getBestCameraToTarget(); - var distance = Math.sqrt(Math.pow(t3d.getX(), 2) + Math.pow(t3d.getY(), 2) + Math.pow(t3d.getZ(), 2)); - if (distance < smallestDistance) - smallestDistance = distance; - } - double poseAmbiguityFactor = estimation.targetsUsed.size() != 1 - ? 1 - : Math.max( - 1, - (estimation.targetsUsed.get(0).getPoseAmbiguity() - + VisionConfig.POSE_AMBIGUITY_SHIFTER) - * VisionConfig.POSE_AMBIGUITY_MULTIPLIER); - double confidenceMultiplier = Math.max( - 1, - (Math.max( - 1, - Math.max(0, smallestDistance - VisionConfig.NOISY_DISTANCE_METERS) - * VisionConfig.DISTANCE_WEIGHT) - * poseAmbiguityFactor) - / (1 - + ((estimation.targetsUsed.size() - 1) * VisionConfig.TAG_PRESENCE_WEIGHT))); - - return VisionConfig.VISION_MEASUREMENT_STANDARD_DEVIATIONS.times(confidenceMultiplier); - } - - public void estimatorChecker(PhotonRunnable estimator) { - var cameraPose = estimator.grabLatestEstimatedPose(); - if (cameraPose != null) { - // New pose from vision - var pose2d = cameraPose.estimatedPose.toPose2d(); - if (originPosition == kRedAllianceWallRightSide) { - pose2d = flipAlliance(pose2d); - } - poseEstimator.addVisionMeasurement(pose2d, cameraPose.timestampSeconds, - confidenceCalculator(cameraPose)); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/TriConsumer.java b/src/main/java/frc/robot/vision/TriConsumer.java new file mode 100644 index 0000000..058f32c --- /dev/null +++ b/src/main/java/frc/robot/vision/TriConsumer.java @@ -0,0 +1,19 @@ +package frc.robot.vision; +// Copyright (c) FRC 1076 PiHi Samurai +// You may use, distribute, and modify this software under the terms of +// the license found in the root directory of this project + +import java.util.Objects; + +@FunctionalInterface +public interface TriConsumer { + public abstract void accept(T t, U u, V v); + + public default TriConsumer andThen(TriConsumer after) { + Objects.requireNonNull(after); + return (t, u, v) -> { + this.accept(t, u, v); + after.accept(t, u, v); + }; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index 6dc753f..0a075cf 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -7,11 +7,16 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.List; +import java.util.Vector; import java.util.stream.Collector; import java.util.stream.Collectors; import java.util.stream.Stream; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -36,149 +41,239 @@ import frc.robot.drivetrain.TunerConstants; public class VisionConfig { + + public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + public static final String driverCamName = "DRIVER_CAM"; //PV name of the driver camera + + public static final edu.wpi.first.math.Vector kDefaultSingleTagStdDevs = VecBuilder.fill(1, 1, 1); + public static final edu.wpi.first.math.Vector kDefaultMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 0.5); + + public static enum PhotonConfig { + + FRONT_LEFT_CAM( + "FRONT_LEFT_CAM", + kDefaultSingleTagStdDevs, + kDefaultMultiTagStdDevs, + PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, + PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, + 12.389, 11.683, 11.513639, // 15 - 7.163, 15 - 2.892, 19.162, + 0, 0, -20 //11.385, 17.961, -40 + ), + FRONT_RIGHT_CAM( + "FRONT_RIGHT_CAM", + kDefaultSingleTagStdDevs, + kDefaultMultiTagStdDevs, + PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, + PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, + 12.389, -11.683, 11.513639, // 15 - 7.163, -(15 - 2.892), 19.162, + 0, 0, 20 // -11.385, 17.961, 40 + ); + + public final String name; + public final Transform3d offset; + public final Matrix defaultSingleTagStdDevs; + public final Matrix defaultMultiTagStdDevs; + public final PoseStrategy multiTagPoseStrategy; + public final PoseStrategy singleTagPoseStrategy; + private PhotonConfig( + String name, + Matrix defaultSingleTagStdDevs, + Matrix defaultMultiTagStdDevs, + PoseStrategy multiTagPoseStrategy, + PoseStrategy singleTagPoseStrategy, + double xInch, double yInch, double zInch, + double rollDeg, double pitchDeg, double yawDeg + ) { + this.name = name; + this.offset = new Transform3d( + Units.inchesToMeters(xInch), + Units.inchesToMeters(yInch), + Units.inchesToMeters(zInch), + new Rotation3d( + Units.degreesToRadians(rollDeg), + Units.degreesToRadians(pitchDeg), + Units.degreesToRadians(yawDeg) + ) + ); + this.multiTagPoseStrategy = multiTagPoseStrategy; + this.singleTagPoseStrategy = singleTagPoseStrategy; + this.defaultMultiTagStdDevs = defaultMultiTagStdDevs; + this.defaultSingleTagStdDevs = defaultSingleTagStdDevs; + } + } + + public class PathPlannerConstants { + public static final PathConstraints pathConstraints = new PathConstraints(2, 2, Units.degreesToRadians(360), Units.degreesToRadians(360)); + public static final Transform2d robotOffset = new Transform2d(0.508, 0, Rotation2d.kZero); + public static final double pathGenerationToleranceMeters = 0.011; // Technically it's anything larger than 0.01, but I'm adding .001 just to be safe + public static final double LEDpathToleranceMeters = 0.03; + + public static class Control { + public static final PIDConstants transPID = new PIDConstants(5, 0, 0); + public static final PIDConstants rotPID = new PIDConstants(5, 0, 0); + } + } + + public final class GeometryUtils { + + /** rotates a Pose2d */ + public static Pose2d rotatePose(Pose2d pose, Rotation2d rot) { + return new Pose2d(pose.getTranslation(), pose.getRotation().rotateBy(rot)); + } - // Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard - public static final int TOTAL_CAMS = 2; //TODO: change to 4 where 4 cams are available - public static final String[] CAM_NAMES = new String[] {"Left_Cam", "Right_Cam"}; //TODO: add center cam and drive cam - - //Camera Positions - public static final Transform3d[] ROBOT_TO_CAM_TRANSFORMS = new Transform3d[] { - //left cam - new Transform3d( - new Translation3d(Units.inchesToMeters(11.882),Units.inchesToMeters(11.020),Units.inchesToMeters(6.767)), - new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(-20))), - //right cam - new Transform3d( - new Translation3d(Units.inchesToMeters(11.882),Units.inchesToMeters(-11.020),Units.inchesToMeters(6.767)), - new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(20))) - }; - - // Creates field layout for AprilTags - public static AprilTagFieldLayout TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - // Standard deviation of vision poses, this helps with correction or something idk thats what photon said - // TODO: experiment with standard deviation values and set them to whatever gives the most correct pose - public static final Matrix SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8); // TODO: example values, change when testing - public static final Matrix MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1); //TODO: change values when testing - - public static final double AMBIGUITY_THRESHOLD = 0.2; - public static final Distance SINGLE_TAG_DISTANCE_THRESHOLD = Meters.of(4.5); - - public static final Distance FIELD_LENGTH = Meters.of(17.548); - public static final Distance FIELD_WIDTH = Meters.of(8.052); - - public static final double FIELD_LENGTH_METERS = 17.548; - public static final double FIELD_WIDTH_METERS = 8.052; - - public static boolean USE_VISION = true; - - - public static final double APRILTAG_AMBIGUITY_THRESHOLD = 0.2; - public static final double POSE_AMBIGUITY_SHIFTER = 0.2; - public static final double POSE_AMBIGUITY_MULTIPLIER = 4; - public static final double NOISY_DISTANCE_METERS = 2.5; - public static final double DISTANCE_WEIGHT = 7; - public static final int TAG_PRESENCE_WEIGHT = 10; - - - /** - * Standard deviations of model states. Increase these numbers to trust your - * model's state estimates less. This - * matrix is in the form [x, y, theta]ᵀ, with units in meters and radians, then - * meters. - */ - public static final Matrix VISION_MEASUREMENT_STANDARD_DEVIATIONS = VecBuilder.fill(.05, .05, Units.degreesToRadians(5)); - - /** - * Standard deviations of the vision measurements. Increase these numbers to - * trust global measurements from vision - * less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and - * radians. - */ - public static final Matrix STATE_STANDARD_DEVIATIONS = VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(2.5)); - - - public static class AlignmentConfig { - public static final double THETA_kP = 3.0; - public static final double THETA_kI = 0.0; - public static final double THETA_kD = 0.0; + /* finds angle from one pose to another pose */ + public static Rotation2d angleToPose(Pose2d startPose, Pose2d endPose){ + return endPose.getTranslation().minus(startPose.getTranslation()).getAngle(); + } + } + + public static final double kOdometryUpdateFrequency = 50.0; - public static final double X_kP = 5.0; - public static final double X_kI = 0.0; - public static final double X_kD = 0.0; + // // Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard + // public static final int TOTAL_CAMS = 2; //TODO: change to 4 where 4 cams are available + // public static final String[] CAM_NAMES = new String[] {"Left_Cam", "Right_Cam"}; //TODO: add center cam and drive cam + + // //Camera Positions + // public static final Transform3d[] ROBOT_TO_CAM_TRANSFORMS = new Transform3d[] { + // //left cam + // new Transform3d( + // new Translation3d(Units.inchesToMeters(11.882),Units.inchesToMeters(11.020),Units.inchesToMeters(6.767)), + // new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(-20))), + // //right cam + // new Transform3d( + // new Translation3d(Units.inchesToMeters(11.882),Units.inchesToMeters(-11.020),Units.inchesToMeters(6.767)), + // new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(20))) + // }; + + // // Creates field layout for AprilTags + // public static AprilTagFieldLayout TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + // // Standard deviation of vision poses, this helps with correction or something idk thats what photon said + // // TODO: experiment with standard deviation values and set them to whatever gives the most correct pose + // public static final Matrix SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8); // TODO: example values, change when testing + // public static final Matrix MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1); //TODO: change values when testing + + // public static final double AMBIGUITY_THRESHOLD = 0.2; + // public static final Distance SINGLE_TAG_DISTANCE_THRESHOLD = Meters.of(4.5); + + // public static final Distance FIELD_LENGTH = Meters.of(17.548); + // public static final Distance FIELD_WIDTH = Meters.of(8.052); + + // public static final double FIELD_LENGTH_METERS = 17.548; + // public static final double FIELD_WIDTH_METERS = 8.052; + + // public static boolean USE_VISION = true; + + + // public static final double APRILTAG_AMBIGUITY_THRESHOLD = 0.2; + // public static final double POSE_AMBIGUITY_SHIFTER = 0.2; + // public static final double POSE_AMBIGUITY_MULTIPLIER = 4; + // public static final double NOISY_DISTANCE_METERS = 2.5; + // public static final double DISTANCE_WEIGHT = 7; + // public static final int TAG_PRESENCE_WEIGHT = 10; + + + // /** + // * Standard deviations of model states. Increase these numbers to trust your + // * model's state estimates less. This + // * matrix is in the form [x, y, theta]ᵀ, with units in meters and radians, then + // * meters. + // */ + // public static final Matrix VISION_MEASUREMENT_STANDARD_DEVIATIONS = VecBuilder.fill(.05, .05, Units.degreesToRadians(5)); + + // /** + // * Standard deviations of the vision measurements. Increase these numbers to + // * trust global measurements from vision + // * less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and + // * radians. + // */ + // public static final Matrix STATE_STANDARD_DEVIATIONS = VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(2.5)); + + + // public static class AlignmentConfig { + // public static final double THETA_kP = 3.0; + // public static final double THETA_kI = 0.0; + // public static final double THETA_kD = 0.0; - public static final double Y_kP = 5.0; - public static final double Y_kI = 0.0; - public static final double Y_kD = 0.0; + // public static final double X_kP = 5.0; + // public static final double X_kI = 0.0; + // public static final double X_kD = 0.0; + + // public static final double Y_kP = 5.0; + // public static final double Y_kI = 0.0; + // public static final double Y_kD = 0.0; - public static final Distance DISTANCE_TOLERANCE = Inches.of(0.5); - public static final Distance LATERAL_TOLERANCE = Inches.of(1.0); - public static final double THETA_TOLERANCE = 0.03; - - public static final Distance ALIGNMENT_TOLERANCE = Inches.of(0.5); //inches - public static final LinearVelocity MAX_ALIGN_TRANSLATION_VELOCITY = TunerConstants.kSpeedAt12Volts.div(2); - public static final LinearAcceleration MAX_ALIGN_TRANSLATION_ACCELERATION = MetersPerSecondPerSecond.of(6.0); - public static final AngularVelocity MAX_ALIGN_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75); - public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(6.0 * Math.PI); + // public static final Distance DISTANCE_TOLERANCE = Inches.of(0.5); + // public static final Distance LATERAL_TOLERANCE = Inches.of(1.0); + // public static final double THETA_TOLERANCE = 0.03; + + // public static final Distance ALIGNMENT_TOLERANCE = Inches.of(0.5); //inches + // public static final LinearVelocity MAX_ALIGN_TRANSLATION_VELOCITY = TunerConstants.kSpeedAt12Volts.div(2); + // public static final LinearAcceleration MAX_ALIGN_TRANSLATION_ACCELERATION = MetersPerSecondPerSecond.of(6.0); + // public static final AngularVelocity MAX_ALIGN_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75); + // public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(6.0 * Math.PI); - //April Tag IDs - public static final double id1 = 1; - public static final double id2 = 2; - public static final double id3 = 3; - public static final double id4 = 4; - public static final double id5 = 5; - public static final double id6 = 6; - public static final double id7 = 7; - public static final double id8 = 8; - public static final double id9 = 9; - public static final double id10 = 10; - public static final double id11 = 11; - public static final double id12 = 12; - public static final double id13 = 13; - public static final double id14 = 14; - public static final double id15 = 15; - public static final double id16 = 16; - public static final double id17 = 17; - public static final double id18 = 18; - public static final double id19 = 19; - public static final double id20 = 20; - public static final double id21 = 21; - public static final double id22 = 22; - - //Joey's Pose Constants for the Reef Locations - public static final Pose2d Error = new Pose2d(6, 6, Rotation2d.fromDegrees(0)); - - public static final Pose2d Ablue = new Pose2d(3.180, 4.175, Rotation2d.fromDegrees(0)); - public static final Pose2d Bblue = new Pose2d(3.180, 3.850, Rotation2d.fromDegrees(0)); - public static final Pose2d Cblue = new Pose2d(3.685, 2.975, Rotation2d.fromDegrees(60)); - public static final Pose2d Dblue = new Pose2d(3.975, 2.825, Rotation2d.fromDegrees(60)); - public static final Pose2d Eblue = new Pose2d(5.000, 2.825, Rotation2d.fromDegrees(120)); - public static final Pose2d Fblue = new Pose2d(5.285, 2.975, Rotation2d.fromDegrees(120)); - public static final Pose2d Gblue = new Pose2d(5.8, 3.850, Rotation2d.fromDegrees(180)); - public static final Pose2d Hblue = new Pose2d(5.8, 4.175, Rotation2d.fromDegrees(180)); - public static final Pose2d Iblue = new Pose2d(5.285, 5.075, Rotation2d.fromDegrees(240)); - public static final Pose2d Jblue = new Pose2d(5.000, 5.230, Rotation2d.fromDegrees(240)); - public static final Pose2d Kblue = new Pose2d(3.975, 5.230, Rotation2d.fromDegrees(300)); - public static final Pose2d Lblue = new Pose2d(3.685, 5.075, Rotation2d.fromDegrees(300)); - - public static final double fieldFlip = 17.5; - public static final double fieldFlipy = 8; - - public static final Pose2d Ared = new Pose2d(fieldFlip - 3.180, fieldFlipy - 4.175, Rotation2d.fromDegrees(180)); - public static final Pose2d Bred = new Pose2d(fieldFlip - 3.180, fieldFlipy - 3.850, Rotation2d.fromDegrees(180)); - public static final Pose2d Cred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 2.975, Rotation2d.fromDegrees(-120)); - public static final Pose2d Dred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 2.825, Rotation2d.fromDegrees(-120)); - public static final Pose2d Ered = new Pose2d(fieldFlip - 5.000, fieldFlipy - 2.825, Rotation2d.fromDegrees(-60)); - public static final Pose2d Fred = new Pose2d(fieldFlip - 5.285, fieldFlipy - 2.975, Rotation2d.fromDegrees(-60)); - public static final Pose2d Gred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 3.850, Rotation2d.fromDegrees(0)); - public static final Pose2d Hred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 4.175, Rotation2d.fromDegrees(0)); - public static final Pose2d Ired = new Pose2d(fieldFlip - 5.285, fieldFlipy - 5.075, Rotation2d.fromDegrees(-300)); - public static final Pose2d Jred = new Pose2d(fieldFlip - 5.000, fieldFlipy - 5.230, Rotation2d.fromDegrees(-300)); - public static final Pose2d Kred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 5.230, Rotation2d.fromDegrees(-240)); - public static final Pose2d Lred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 5.075, Rotation2d.fromDegrees(-240)); - - - //TODO: MIGHT NEED TO PLAY AROUND WITH ALL THE POSES BELOW + // //April Tag IDs + // public static final double id1 = 1; + // public static final double id2 = 2; + // public static final double id3 = 3; + // public static final double id4 = 4; + // public static final double id5 = 5; + // public static final double id6 = 6; + // public static final double id7 = 7; + // public static final double id8 = 8; + // public static final double id9 = 9; + // public static final double id10 = 10; + // public static final double id11 = 11; + // public static final double id12 = 12; + // public static final double id13 = 13; + // public static final double id14 = 14; + // public static final double id15 = 15; + // public static final double id16 = 16; + // public static final double id17 = 17; + // public static final double id18 = 18; + // public static final double id19 = 19; + // public static final double id20 = 20; + // public static final double id21 = 21; + // public static final double id22 = 22; + + // //Joey's Pose Constants for the Reef Locations + // public static final Pose2d Error = new Pose2d(6, 6, Rotation2d.fromDegrees(0)); + + // public static final Pose2d Ablue = new Pose2d(3.180, 4.175, Rotation2d.fromDegrees(0)); + // public static final Pose2d Bblue = new Pose2d(3.180, 3.850, Rotation2d.fromDegrees(0)); + // public static final Pose2d Cblue = new Pose2d(3.685, 2.975, Rotation2d.fromDegrees(60)); + // public static final Pose2d Dblue = new Pose2d(3.975, 2.825, Rotation2d.fromDegrees(60)); + // public static final Pose2d Eblue = new Pose2d(5.000, 2.825, Rotation2d.fromDegrees(120)); + // public static final Pose2d Fblue = new Pose2d(5.285, 2.975, Rotation2d.fromDegrees(120)); + // public static final Pose2d Gblue = new Pose2d(5.8, 3.850, Rotation2d.fromDegrees(180)); + // public static final Pose2d Hblue = new Pose2d(5.8, 4.175, Rotation2d.fromDegrees(180)); + // public static final Pose2d Iblue = new Pose2d(5.285, 5.075, Rotation2d.fromDegrees(240)); + // public static final Pose2d Jblue = new Pose2d(5.000, 5.230, Rotation2d.fromDegrees(240)); + // public static final Pose2d Kblue = new Pose2d(3.975, 5.230, Rotation2d.fromDegrees(300)); + // public static final Pose2d Lblue = new Pose2d(3.685, 5.075, Rotation2d.fromDegrees(300)); + + // public static final double fieldFlip = 17.5; + // public static final double fieldFlipy = 8; + + // public static final Pose2d Ared = new Pose2d(fieldFlip - 3.180, fieldFlipy - 4.175, Rotation2d.fromDegrees(180)); + // public static final Pose2d Bred = new Pose2d(fieldFlip - 3.180, fieldFlipy - 3.850, Rotation2d.fromDegrees(180)); + // public static final Pose2d Cred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 2.975, Rotation2d.fromDegrees(-120)); + // public static final Pose2d Dred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 2.825, Rotation2d.fromDegrees(-120)); + // public static final Pose2d Ered = new Pose2d(fieldFlip - 5.000, fieldFlipy - 2.825, Rotation2d.fromDegrees(-60)); + // public static final Pose2d Fred = new Pose2d(fieldFlip - 5.285, fieldFlipy - 2.975, Rotation2d.fromDegrees(-60)); + // public static final Pose2d Gred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 3.850, Rotation2d.fromDegrees(0)); + // public static final Pose2d Hred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 4.175, Rotation2d.fromDegrees(0)); + // public static final Pose2d Ired = new Pose2d(fieldFlip - 5.285, fieldFlipy - 5.075, Rotation2d.fromDegrees(-300)); + // public static final Pose2d Jred = new Pose2d(fieldFlip - 5.000, fieldFlipy - 5.230, Rotation2d.fromDegrees(-300)); + // public static final Pose2d Kred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 5.230, Rotation2d.fromDegrees(-240)); + // public static final Pose2d Lred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 5.075, Rotation2d.fromDegrees(-240)); + + + + + //TODO: MIGHT NEED TO PLAY AROUND WITH ALL THE POSES BELOW // /** Pose of the robot relative to a reef branch for scoring coral on L4 */ // public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L4 = new Transform2d( @@ -381,5 +476,4 @@ public static class AlignmentConfig { // public static final Distance LATERAL_TARGET_L4_LEFT = Meters.of(0.05); // public static final Distance LATERAL_TARGET_L4_RIGHT = Meters.of(0.02); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionLocalizationSystem.java b/src/main/java/frc/robot/vision/VisionLocalizationSystem.java new file mode 100644 index 0000000..7698334 --- /dev/null +++ b/src/main/java/frc/robot/vision/VisionLocalizationSystem.java @@ -0,0 +1,113 @@ +// Copyright (c) FRC 1076 PiHi Samurai +// You may use, distribute, and modify this software under the terms of +// the license found in the root directory of this project + +package frc.robot.vision; + +import java.util.HashMap; +import java.util.Map; +import org.photonvision.PhotonPoseEstimator; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + + +/** + * A class that manages robot position estimates from multiple cameras and + * sends them to all externally registered consumers using the {@link VisionLocalizationSystem#update()} method + *

Consumers are registered using the {@link VisionLocalizationSystem#registerMeasurementConsumer()} method + */ +public class VisionLocalizationSystem { + + private class CamStruct { + public final CamLocalizer camera; + public boolean cameraActive = true; //Signals whether or not the camera reading should be added to vision measurements + + /** Sets whether or not the camera is active */ + public void setActive(boolean active) { + cameraActive = active; + } + + public CamStruct(CamLocalizer camera) { + this.camera = camera; + } + } + + //A Consumer that accepts a Pose3d and a Matrix of Standard Deviations, usually should call addVisionMeasurements() on a SwerveDrivePoseEstimator3d + private TriConsumer> measurementConsumer = (pose, timestamp, stddevs) -> {}; //A default no-op consumer is instantiated to prevent null pointer dereferences + + private final Map cameras = new HashMap<>(); + + /** + * Registers a consumer + *

This method should be called externally on initialization + * + * @param consumer A consumer that accepts a Pose2d, Double, and 3x1 Matrix + *

The Pose2d is the robot pose estimate in meters + *

The Double is the timestamp of the measurement in seconds + *

The 3x1 Matrix is the standard deviations of the measurement + */ + public void registerMeasurementConsumer(TriConsumer> consumer) { + measurementConsumer = measurementConsumer.andThen(consumer); + } + + /** + * Adds a camera to the vision system + */ + public void addCamera(CamLocalizer camera) { + CamStruct camStruct = new CamStruct(camera); + cameras.put(camera.getName(), camStruct); + } + + /** + * Enables or disables selected cameras + * @param enabled Whether or not the cameras should be enabled + * @param cams The IDs of the cameras to enable or disable + */ + public void enableCameras(boolean enabled, String... cams) { + for (String camID : cams) { + cameras.get(camID).setActive(enabled); + } + } + + /** + * Enables or disables all cameras + * @param enabled Whether or not the cameras should be enabled + */ + public void enableAllCameras(boolean enabled) { + for (var camStruct : cameras.values()) { + camStruct.setActive(enabled); + } + } + + /** + * For PhotonVision cameras, this method will set the pose estimator strategy from {@link PhotonPoseEstimator#PoseStrategy} + * For Limelight cameras, this method will call the default method, which does nothing + */ + public void setPhotonPoseStrategy(PhotonPoseEstimator.PoseStrategy strategy, String... cams){ + for (String camID : cams) { + cameras.get(camID).camera.setPoseStrategy(strategy); + } + } + + /** + * Fetches pose estimate from cameras and sends them to all consumers. This function should be called exactly once every main function loop + */ + public void update() { + for (var camStruct : cameras.values()) { + if (camStruct.cameraActive) { + camStruct.camera.getPoseEstimate().ifPresent( + (estimate) -> { + measurementConsumer.accept( + estimate.pose(), + estimate.timestampSeconds(), + estimate.stdDevs() + ); + } + ); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java index 4cb8529..4c77055 100644 --- a/src/main/java/frc/robot/vision/commands/AutoAlign.java +++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java @@ -1,319 +1,319 @@ -package frc.robot.vision.commands; +// package frc.robot.vision.commands; -import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.vision.VisionConfig.*; -import static frc.robot.vision.VisionConfig.AlignmentConfig.*; +// import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide; +// import static edu.wpi.first.units.Units.Meters; +// import static edu.wpi.first.units.Units.MetersPerSecond; +// import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +// import static edu.wpi.first.units.Units.RadiansPerSecond; +// import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +// import static edu.wpi.first.units.Units.RotationsPerSecond; +// import static frc.robot.vision.VisionConfig.*; +// import static frc.robot.vision.VisionConfig.AlignmentConfig.*; -import org.photonvision.PhotonCamera; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotContainer; -import frc.robot.drivetrain.CommandSwerveDrivetrain; -import frc.robot.vision.PoseEstimatorSubsystem; -import frc.robot.vision.VisionConfig; +// import org.photonvision.PhotonCamera; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.trajectory.TrapezoidProfile; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.units.measure.LinearVelocity; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.RobotContainer; +// import frc.robot.drivetrain.CommandSwerveDrivetrain; +// import frc.robot.vision.PoseEstimatorSubsystem; +// import frc.robot.vision.VisionConfig; -public class AutoAlign extends Command { - private double x; - private double y; - private LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); - private PhotonCamera leftCam = new PhotonCamera(CAM_NAMES[0]); - private int tagID; +// public class AutoAlign extends Command { +// private double x; +// private double y; +// private LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); +// private PhotonCamera leftCam = new PhotonCamera(CAM_NAMES[0]); +// private int tagID; - private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( - VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), - VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); +// private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( +// VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), +// VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); - private static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints( - VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), - VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); +// private static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints( +// VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), +// VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); - private static final ProfiledPIDController xDistanceController = new ProfiledPIDController( - VisionConfig.AlignmentConfig.X_kP, - VisionConfig.AlignmentConfig.X_kI, - VisionConfig.AlignmentConfig.X_kD, - TRANSLATION_CONSTRAINTS); +// private static final ProfiledPIDController xDistanceController = new ProfiledPIDController( +// VisionConfig.AlignmentConfig.X_kP, +// VisionConfig.AlignmentConfig.X_kI, +// VisionConfig.AlignmentConfig.X_kD, +// TRANSLATION_CONSTRAINTS); - private static final ProfiledPIDController yDistanceController = new ProfiledPIDController( - VisionConfig.AlignmentConfig.Y_kP, - VisionConfig.AlignmentConfig.Y_kI, - VisionConfig.AlignmentConfig.Y_kD, - TRANSLATION_CONSTRAINTS); +// private static final ProfiledPIDController yDistanceController = new ProfiledPIDController( +// VisionConfig.AlignmentConfig.Y_kP, +// VisionConfig.AlignmentConfig.Y_kI, +// VisionConfig.AlignmentConfig.Y_kD, +// TRANSLATION_CONSTRAINTS); - private static final ProfiledPIDController thetaController = new ProfiledPIDController( - VisionConfig.AlignmentConfig.THETA_kP, - VisionConfig.AlignmentConfig.THETA_kI, - VisionConfig.AlignmentConfig.THETA_kD, - THETA_CONSTRAINTS); +// private static final ProfiledPIDController thetaController = new ProfiledPIDController( +// VisionConfig.AlignmentConfig.THETA_kP, +// VisionConfig.AlignmentConfig.THETA_kI, +// VisionConfig.AlignmentConfig.THETA_kD, +// THETA_CONSTRAINTS); - private static Pose2d currentPose; - private static Pose2d targetPose; - private static boolean commandRan = false; +// private static Pose2d currentPose; +// private static Pose2d targetPose; +// private static boolean commandRan = false; - public AutoAlign(boolean left) { - //set tolerances of all PID controllers - xDistanceController.setTolerance(VisionConfig.AlignmentConfig.DISTANCE_TOLERANCE.in(Meters)); - xDistanceController.setIntegratorRange(-.15, .15); - yDistanceController.setTolerance(VisionConfig.AlignmentConfig.LATERAL_TOLERANCE.in(Meters)); - yDistanceController.setIntegratorRange(-.15, .15); - thetaController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(180)); +// public AutoAlign(boolean left) { +// //set tolerances of all PID controllers +// xDistanceController.setTolerance(VisionConfig.AlignmentConfig.DISTANCE_TOLERANCE.in(Meters)); +// xDistanceController.setIntegratorRange(-.15, .15); +// yDistanceController.setTolerance(VisionConfig.AlignmentConfig.LATERAL_TOLERANCE.in(Meters)); +// yDistanceController.setIntegratorRange(-.15, .15); +// thetaController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(180)); - currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); - targetPose = getTargetReefPose(left); +// currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); +// targetPose = getTargetReefPose(left); - xDistanceController.reset(0); - yDistanceController.reset(0); - thetaController.reset(0); +// xDistanceController.reset(0); +// yDistanceController.reset(0); +// thetaController.reset(0); - addRequirements(CommandSwerveDrivetrain.getInstance()); - } +// addRequirements(CommandSwerveDrivetrain.getInstance()); +// } - public double getDistanceFromTag(Pose2d goalPose) { - double xDiff = goalPose.getX() - currentPose.getX(); - double yDiff = goalPose.getY() - currentPose.getY(); - double distance = Math.sqrt(Math.pow(xDiff, 2) + Math.pow(yDiff, 2)); - return distance; - } +// public double getDistanceFromTag(Pose2d goalPose) { +// double xDiff = goalPose.getX() - currentPose.getX(); +// double yDiff = goalPose.getY() - currentPose.getY(); +// double distance = Math.sqrt(Math.pow(xDiff, 2) + Math.pow(yDiff, 2)); +// return distance; +// } - private Pose2d getTargetReefPose(boolean left){ - var photonResult = leftCam.getAllUnreadResults(); - var currAlliance = DriverStation.getAlliance(); +// private Pose2d getTargetReefPose(boolean left){ +// var photonResult = leftCam.getAllUnreadResults(); +// var currAlliance = DriverStation.getAlliance(); - for (var result : photonResult){ - if (result.hasTargets()){ - tagID = result.getBestTarget().fiducialId; - } - } +// for (var result : photonResult){ +// if (result.hasTargets()){ +// tagID = result.getBestTarget().fiducialId; +// } +// } - if (currAlliance.get() == Alliance.Blue){ - // IF(LEFT) - // { - // CLOSESTDIST = NULL; - // TARGETPOSE = A; - // TEMPdIST = DISTANCE(CURPOS, TARGETPOSE); - // CLOSESTDIST = TEMPDIST; - // POSE = A +// if (currAlliance.get() == Alliance.Blue){ +// // IF(LEFT) +// // { +// // CLOSESTDIST = NULL; +// // TARGETPOSE = A; +// // TEMPdIST = DISTANCE(CURPOS, TARGETPOSE); +// // CLOSESTDIST = TEMPDIST; +// // POSE = A - // TARGETPOSE = C; - // TEMPdIST = DISTANCE(CURPOS, TARGETPOSE) - // IF(TEMPDIST < CLOSESTDIST) - // POSE = C; +// // TARGETPOSE = C; +// // TEMPdIST = DISTANCE(CURPOS, TARGETPOSE) +// // IF(TEMPDIST < CLOSESTDIST) +// // POSE = C; - // } - if (left) { - double min = getDistanceFromTag(Ablue); - targetPose = Ablue; +// // } +// if (left) { +// double min = getDistanceFromTag(Ablue); +// targetPose = Ablue; - if (min > getDistanceFromTag(Cblue)){ - min = getDistanceFromTag(Cblue); - targetPose = Cblue; - } - if (min > getDistanceFromTag(Kblue)) { - min = getDistanceFromTag(Kblue); - targetPose = Kblue; - } - if (min > getDistanceFromTag(Iblue)) { - min = getDistanceFromTag(Iblue); - targetPose = Iblue; - } - if (min > getDistanceFromTag(Gblue)) { - min = getDistanceFromTag(Gblue); - targetPose = Gblue; - } - if (min > getDistanceFromTag(Eblue)) { - min = getDistanceFromTag(Eblue); - targetPose = Eblue; - } - } else { - double min = getDistanceFromTag(Bblue); - targetPose = Ablue; +// if (min > getDistanceFromTag(Cblue)){ +// min = getDistanceFromTag(Cblue); +// targetPose = Cblue; +// } +// if (min > getDistanceFromTag(Kblue)) { +// min = getDistanceFromTag(Kblue); +// targetPose = Kblue; +// } +// if (min > getDistanceFromTag(Iblue)) { +// min = getDistanceFromTag(Iblue); +// targetPose = Iblue; +// } +// if (min > getDistanceFromTag(Gblue)) { +// min = getDistanceFromTag(Gblue); +// targetPose = Gblue; +// } +// if (min > getDistanceFromTag(Eblue)) { +// min = getDistanceFromTag(Eblue); +// targetPose = Eblue; +// } +// } else { +// double min = getDistanceFromTag(Bblue); +// targetPose = Ablue; - if (min > getDistanceFromTag(Dblue)){ - min = getDistanceFromTag(Dblue); - targetPose = Dblue; - } - if (min > getDistanceFromTag(Lblue)) { - min = getDistanceFromTag(Lblue); - targetPose = Lblue; - } - if (min > getDistanceFromTag(Jblue)) { - min = getDistanceFromTag(Jblue); - targetPose = Jblue; - } - if (min > getDistanceFromTag(Hblue)) { - min = getDistanceFromTag(Hblue); - targetPose = Hblue; - } - if (min > getDistanceFromTag(Fblue)) { - min = getDistanceFromTag(Fblue); - targetPose = Fblue; - } - } +// if (min > getDistanceFromTag(Dblue)){ +// min = getDistanceFromTag(Dblue); +// targetPose = Dblue; +// } +// if (min > getDistanceFromTag(Lblue)) { +// min = getDistanceFromTag(Lblue); +// targetPose = Lblue; +// } +// if (min > getDistanceFromTag(Jblue)) { +// min = getDistanceFromTag(Jblue); +// targetPose = Jblue; +// } +// if (min > getDistanceFromTag(Hblue)) { +// min = getDistanceFromTag(Hblue); +// targetPose = Hblue; +// } +// if (min > getDistanceFromTag(Fblue)) { +// min = getDistanceFromTag(Fblue); +// targetPose = Fblue; +// } +// } - // //DISTANCE FROM - // if (targetPose == Cblue){ - // if (left) { - // return Cblue; - // } else if (!left) { - // return Dblue; - // } - // } else if (getDistanceFromTag(Ablue) < getDistanceFromTag(Kblue)) { - // if (left) { - // return Ablue; - // } else if (!left) { - // return Bblue; - // } - // } else if (getDistanceFromTag(Kblue) < getDistanceFromTag(Iblue)) { - // if (left) { - // return Kblue; - // } else if (!left) { - // return Lblue; - // } - // } else if (getDistanceFromTag(Iblue) < getDistanceFromTag(Gblue)) { - // if (left) { - // return Iblue; - // } else if (!left) { - // return Jblue; - // } - // } else if (getDistanceFromTag(Gblue) < getDistanceFromTag(Eblue)) { - // if (left) { - // return Gblue; - // } else if (!left) { - // return Hblue; - // } - // } else if (getDistanceFromTag(Eblue) < getDistanceFromTag(Cblue)) { - // if (left) { - // return Eblue; - // } else if (!left) { - // return Fblue; - // } - // } +// // //DISTANCE FROM +// // if (targetPose == Cblue){ +// // if (left) { +// // return Cblue; +// // } else if (!left) { +// // return Dblue; +// // } +// // } else if (getDistanceFromTag(Ablue) < getDistanceFromTag(Kblue)) { +// // if (left) { +// // return Ablue; +// // } else if (!left) { +// // return Bblue; +// // } +// // } else if (getDistanceFromTag(Kblue) < getDistanceFromTag(Iblue)) { +// // if (left) { +// // return Kblue; +// // } else if (!left) { +// // return Lblue; +// // } +// // } else if (getDistanceFromTag(Iblue) < getDistanceFromTag(Gblue)) { +// // if (left) { +// // return Iblue; +// // } else if (!left) { +// // return Jblue; +// // } +// // } else if (getDistanceFromTag(Gblue) < getDistanceFromTag(Eblue)) { +// // if (left) { +// // return Gblue; +// // } else if (!left) { +// // return Hblue; +// // } +// // } else if (getDistanceFromTag(Eblue) < getDistanceFromTag(Cblue)) { +// // if (left) { +// // return Eblue; +// // } else if (!left) { +// // return Fblue; +// // } +// // } - } else if (currAlliance.get() == Alliance.Red){ - if (left) { - double min = getDistanceFromTag(Ared); - targetPose = Ared; +// } else if (currAlliance.get() == Alliance.Red){ +// if (left) { +// double min = getDistanceFromTag(Ared); +// targetPose = Ared; - if (min > getDistanceFromTag(Cred)){ - min = getDistanceFromTag(Cred); - targetPose = Cred; - } - if (min > getDistanceFromTag(Kred)) { - min = getDistanceFromTag(Kred); - targetPose = Kblue; - } - if (min > getDistanceFromTag(Ired)) { - min = getDistanceFromTag(Ired); - targetPose = Ired; - } - if (min > getDistanceFromTag(Gred)) { - min = getDistanceFromTag(Gred); - targetPose = Gred; - } - if (min > getDistanceFromTag(Ered)) { - min = getDistanceFromTag(Ered); - targetPose = Ered; - } - } else { - double min = getDistanceFromTag(Bred); - targetPose = Ablue; +// if (min > getDistanceFromTag(Cred)){ +// min = getDistanceFromTag(Cred); +// targetPose = Cred; +// } +// if (min > getDistanceFromTag(Kred)) { +// min = getDistanceFromTag(Kred); +// targetPose = Kblue; +// } +// if (min > getDistanceFromTag(Ired)) { +// min = getDistanceFromTag(Ired); +// targetPose = Ired; +// } +// if (min > getDistanceFromTag(Gred)) { +// min = getDistanceFromTag(Gred); +// targetPose = Gred; +// } +// if (min > getDistanceFromTag(Ered)) { +// min = getDistanceFromTag(Ered); +// targetPose = Ered; +// } +// } else { +// double min = getDistanceFromTag(Bred); +// targetPose = Ablue; - if (min > getDistanceFromTag(Dred)){ - min = getDistanceFromTag(Dred); - targetPose = Dred; - } - if (min > getDistanceFromTag(Lred)) { - min = getDistanceFromTag(Lred); - targetPose = Lred; - } - if (min > getDistanceFromTag(Jred)) { - min = getDistanceFromTag(Jred); - targetPose = Jred; - } - if (min > getDistanceFromTag(Hred)) { - min = getDistanceFromTag(Hred); - targetPose = Hred; - } - if (min > getDistanceFromTag(Fred)) { - min = getDistanceFromTag(Fred); - targetPose = Fred; - } - } +// if (min > getDistanceFromTag(Dred)){ +// min = getDistanceFromTag(Dred); +// targetPose = Dred; +// } +// if (min > getDistanceFromTag(Lred)) { +// min = getDistanceFromTag(Lred); +// targetPose = Lred; +// } +// if (min > getDistanceFromTag(Jred)) { +// min = getDistanceFromTag(Jred); +// targetPose = Jred; +// } +// if (min > getDistanceFromTag(Hred)) { +// min = getDistanceFromTag(Hred); +// targetPose = Hred; +// } +// if (min > getDistanceFromTag(Fred)) { +// min = getDistanceFromTag(Fred); +// targetPose = Fred; +// } +// } - } else if (currAlliance.get() != Alliance.Red && currAlliance.get() != Alliance.Blue){ - System.out.println("ERROR: There's no such thing as purple alliance"); - } +// } else if (currAlliance.get() != Alliance.Red && currAlliance.get() != Alliance.Blue){ +// System.out.println("ERROR: There's no such thing as purple alliance"); +// } - System.out.println("Error: no suitable target pose found (Lineup Command)"); - return targetPose; +// System.out.println("Error: no suitable target pose found (Lineup Command)"); +// return targetPose; - } +// } - @Override - public void execute() { - currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); - // ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; - x = currentPose.getTranslation().getX(); - y = currentPose.getTranslation().getY(); - // double Xvel = currentSpeeds.vxMetersPerSecond; - // double Yvel = currentSpeeds.vyMetersPerSecond; - double XOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(x, targetPose.getX()); - double YOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(y, targetPose.getY()); - double thetaOutput = 0.15 * RotationsPerSecond.of(0.75).in(RadiansPerSecond) * thetaController.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); +// @Override +// public void execute() { +// currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); +// // ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; +// x = currentPose.getTranslation().getX(); +// y = currentPose.getTranslation().getY(); +// // double Xvel = currentSpeeds.vxMetersPerSecond; +// // double Yvel = currentSpeeds.vyMetersPerSecond; +// double XOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(x, targetPose.getX()); +// double YOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(y, targetPose.getY()); +// double thetaOutput = 0.15 * RotationsPerSecond.of(0.75).in(RadiansPerSecond) * thetaController.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); - if (DriverStation.getAlliance().get() == Alliance.Blue){ - CommandSwerveDrivetrain.getInstance().applyRequest(() -> - RobotContainer.drive.withVelocityX(-XOutput) - .withVelocityY(-YOutput) - .withRotationalRate(thetaOutput) +// if (DriverStation.getAlliance().get() == Alliance.Blue){ +// CommandSwerveDrivetrain.getInstance().applyRequest(() -> +// RobotContainer.drive.withVelocityX(-XOutput) +// .withVelocityY(-YOutput) +// .withRotationalRate(thetaOutput) - ).execute(); - } else { - CommandSwerveDrivetrain.getInstance().applyRequest(() -> - RobotContainer.drive.withVelocityX(XOutput) - .withVelocityY(YOutput) - .withRotationalRate(-thetaOutput) - ).execute(); - } +// ).execute(); +// } else { +// CommandSwerveDrivetrain.getInstance().applyRequest(() -> +// RobotContainer.drive.withVelocityX(XOutput) +// .withVelocityY(YOutput) +// .withRotationalRate(-thetaOutput) +// ).execute(); +// } - commandRan = true; - SmartDashboard.putBoolean("command ran", commandRan); - SmartDashboard.putString("Target Pose X", targetPose.toString()); - SmartDashboard.putBoolean("X at target", xDistanceController.atSetpoint()); - SmartDashboard.putBoolean("Y at target", yDistanceController.atSetpoint()); - SmartDashboard.putBoolean("Theta at target", thetaController.atSetpoint()); - //SmartDashboard.putNumber("TAG ID", tagID); +// commandRan = true; +// SmartDashboard.putBoolean("command ran", commandRan); +// SmartDashboard.putString("Target Pose X", targetPose.toString()); +// SmartDashboard.putBoolean("X at target", xDistanceController.atSetpoint()); +// SmartDashboard.putBoolean("Y at target", yDistanceController.atSetpoint()); +// SmartDashboard.putBoolean("Theta at target", thetaController.atSetpoint()); +// //SmartDashboard.putNumber("TAG ID", tagID); - } +// } - @Override - public boolean isFinished() { - return commandRan; - // return xDistanceController.atSetpoint() && yDistanceController.atSetpoint() && thetaController.atSetpoint(); - } +// @Override +// public boolean isFinished() { +// return commandRan; +// // return xDistanceController.atSetpoint() && yDistanceController.atSetpoint() && thetaController.atSetpoint(); +// } - @Override - public void end(boolean interrupted) { +// @Override +// public void end(boolean interrupted) { - } +// } -} +// } diff --git a/src/main/java/frc/robot/vision/commands/DirectDriveToPoseCommand.java b/src/main/java/frc/robot/vision/commands/DirectDriveToPoseCommand.java new file mode 100644 index 0000000..865a96b --- /dev/null +++ b/src/main/java/frc/robot/vision/commands/DirectDriveToPoseCommand.java @@ -0,0 +1,83 @@ +// Copyright (c) FRC 1076 PiHi Samurai +// You may use, distribute, and modify this software under the terms of +// the license found in the root directory of this project + +package frc.robot.vision.commands; + +import java.util.List; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.drivetrain.CommandSwerveDrivetrain; +import frc.robot.vision.VisionConfig.GeometryUtils; +import frc.robot.vision.VisionConfig.PathPlannerConstants; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.IdealStartingState; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; + +/** This automatically drives to a pose without using A* to generate a trajectory, + * useful for when we know there are no obstructions on the field between the robot + * and the desired pose */ +public class DirectDriveToPoseCommand extends Command { + + private Command followPathCommand; + private final Pose2d targetPose; + private final CommandSwerveDrivetrain m_drive; + + public DirectDriveToPoseCommand(CommandSwerveDrivetrain drive, Pose2d targetPose) { + this.m_drive = drive; + this.targetPose = targetPose; + addRequirements(drive); + } + + @Override + public void initialize() { + Pose2d currentPose = m_drive.getPose(); + Pose2d startingWaypoint = new Pose2d(currentPose.getTranslation(), GeometryUtils.angleToPose(currentPose, targetPose)); + + List waypoints = PathPlannerPath.waypointsFromPoses( + startingWaypoint, + targetPose + ); + + // Prevent PathPlanner from treating the start pose as identical to the end pose when they are too close to each other + if (targetPose.getTranslation().getDistance(startingWaypoint.getTranslation()) > PathPlannerConstants.pathGenerationToleranceMeters){ + PathPlannerPath path = new PathPlannerPath( + waypoints, + PathPlannerConstants.pathConstraints, + //new IdealStartingState(m_drive.getVelocityMPS(), m_drive.getHeading()), + null, + new GoalEndState(0, targetPose.getRotation()) + ); + path.preventFlipping = true; + followPathCommand = AutoBuilder.followPath(path); + } else { + followPathCommand = Commands.none(); + } + followPathCommand.schedule(); + } + + public BooleanSupplier atGoal() { + return () -> targetPose.getTranslation().getDistance(m_drive.getPose().getTranslation()) < PathPlannerConstants.LEDpathToleranceMeters; + } + + @Override + public void execute(){ + followPathCommand.execute(); + } + + @Override + public boolean isFinished() { + return followPathCommand.isFinished(); + } + + @Override + public void end(boolean interrupted) { + followPathCommand.end(interrupted); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/commands/LineupCommand.java b/src/main/java/frc/robot/vision/commands/LineupCommand.java index d778490..2a28d36 100644 --- a/src/main/java/frc/robot/vision/commands/LineupCommand.java +++ b/src/main/java/frc/robot/vision/commands/LineupCommand.java @@ -1,256 +1,256 @@ -package frc.robot.vision.commands; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import java.util.Optional; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.google.flatbuffers.Constants; - -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -import frc.robot.vision.VisionConfig; -import frc.robot.RobotContainer; -import frc.robot.drivetrain.*; -import frc.robot.vision.PoseEstimatorSubsystem; - -public class LineupCommand extends Command { - - double x; - double y; - double theta; - LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); - - private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( - VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), - VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); +// package frc.robot.vision.commands; + +// import static edu.wpi.first.units.Units.Meters; +// import static edu.wpi.first.units.Units.MetersPerSecond; +// import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +// import static edu.wpi.first.units.Units.RadiansPerSecond; +// import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +// import static edu.wpi.first.units.Units.RotationsPerSecond; + +// import java.util.Optional; + +// import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +// import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +// import com.ctre.phoenix6.sim.ChassisReference; +// import com.ctre.phoenix6.swerve.SwerveRequest; +// import com.google.flatbuffers.Constants; + +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.math.trajectory.TrapezoidProfile; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.units.measure.LinearVelocity; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; + +// import frc.robot.vision.VisionConfig; +// import frc.robot.RobotContainer; +// import frc.robot.drivetrain.*; +// import frc.robot.vision.PoseEstimatorSubsystem; + +// public class LineupCommand extends Command { + +// double x; +// double y; +// double theta; +// LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); + +// private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( +// VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), +// VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); - private static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints( - VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), - VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); - - - private static final ProfiledPIDController xDistanceController = new ProfiledPIDController( - VisionConfig.AlignmentConfig.X_kP, - VisionConfig.AlignmentConfig.X_kI, - VisionConfig.AlignmentConfig.X_kD, - TRANSLATION_CONSTRAINTS); - - private static final ProfiledPIDController yDistanceController = new ProfiledPIDController( - VisionConfig.AlignmentConfig.Y_kP, - VisionConfig.AlignmentConfig.Y_kI, - VisionConfig.AlignmentConfig.Y_kD, - TRANSLATION_CONSTRAINTS); - - private static final ProfiledPIDController thetaController = new ProfiledPIDController( - VisionConfig.AlignmentConfig.THETA_kP, - VisionConfig.AlignmentConfig.THETA_kI, - VisionConfig.AlignmentConfig.THETA_kD, - THETA_CONSTRAINTS); +// private static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints( +// VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), +// VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); + + +// private static final ProfiledPIDController xDistanceController = new ProfiledPIDController( +// VisionConfig.AlignmentConfig.X_kP, +// VisionConfig.AlignmentConfig.X_kI, +// VisionConfig.AlignmentConfig.X_kD, +// TRANSLATION_CONSTRAINTS); + +// private static final ProfiledPIDController yDistanceController = new ProfiledPIDController( +// VisionConfig.AlignmentConfig.Y_kP, +// VisionConfig.AlignmentConfig.Y_kI, +// VisionConfig.AlignmentConfig.Y_kD, +// TRANSLATION_CONSTRAINTS); + +// private static final ProfiledPIDController thetaController = new ProfiledPIDController( +// VisionConfig.AlignmentConfig.THETA_kP, +// VisionConfig.AlignmentConfig.THETA_kI, +// VisionConfig.AlignmentConfig.THETA_kD, +// THETA_CONSTRAINTS); - private static final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo); +// private static final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() +// .withDriveRequestType(DriveRequestType.Velocity) +// .withSteerRequestType(SteerRequestType.MotionMagicExpo); - private static Pose2d currentPose; - private static Pose2d targetPose; - private static boolean leftReefLineup; - private static boolean commandRan = false; - - public LineupCommand(boolean left) { - //set tolerances of all PID controllers - xDistanceController.setTolerance(VisionConfig.AlignmentConfig.DISTANCE_TOLERANCE.in(Meters)); - xDistanceController.setIntegratorRange(-.15, .15); - yDistanceController.setTolerance(VisionConfig.AlignmentConfig.LATERAL_TOLERANCE.in(Meters)); - yDistanceController.setIntegratorRange(-.15, .15); - thetaController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(180)); - - currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); - targetPose = getTargetReefPose(left); - - xDistanceController.reset(0); - yDistanceController.reset(0); - thetaController.reset(0); - - leftReefLineup = left; - - addRequirements(CommandSwerveDrivetrain.getInstance()); - } - - @Override - public void initialize() { - SmartDashboard.putBoolean("command ran", commandRan); - } - - @Override - public void execute() { - currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); - // ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; - x = currentPose.getTranslation().getX(); - y = currentPose.getTranslation().getY(); - // double Xvel = currentSpeeds.vxMetersPerSecond; - // double Yvel = currentSpeeds.vyMetersPerSecond; - double XOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(x, targetPose.getX()); - double YOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(y, targetPose.getY()); - double thetaOutput = 0.15 * RotationsPerSecond.of(0.75).in(RadiansPerSecond) * thetaController.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); - - if (DriverStation.getAlliance().get() == Alliance.Blue){ - CommandSwerveDrivetrain.getInstance().applyRequest(() -> - robotCentricRequest.withVelocityX(-XOutput) - .withVelocityY(-YOutput) - .withRotationalRate(thetaOutput) +// private static Pose2d currentPose; +// private static Pose2d targetPose; +// private static boolean leftReefLineup; +// private static boolean commandRan = false; + +// public LineupCommand(boolean left) { +// //set tolerances of all PID controllers +// xDistanceController.setTolerance(VisionConfig.AlignmentConfig.DISTANCE_TOLERANCE.in(Meters)); +// xDistanceController.setIntegratorRange(-.15, .15); +// yDistanceController.setTolerance(VisionConfig.AlignmentConfig.LATERAL_TOLERANCE.in(Meters)); +// yDistanceController.setIntegratorRange(-.15, .15); +// thetaController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(180)); + +// currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); +// targetPose = getTargetReefPose(left); + +// xDistanceController.reset(0); +// yDistanceController.reset(0); +// thetaController.reset(0); + +// leftReefLineup = left; + +// addRequirements(CommandSwerveDrivetrain.getInstance()); +// } + +// @Override +// public void initialize() { +// SmartDashboard.putBoolean("command ran", commandRan); +// } + +// @Override +// public void execute() { +// currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); +// // ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; +// x = currentPose.getTranslation().getX(); +// y = currentPose.getTranslation().getY(); +// // double Xvel = currentSpeeds.vxMetersPerSecond; +// // double Yvel = currentSpeeds.vyMetersPerSecond; +// double XOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(x, targetPose.getX()); +// double YOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(y, targetPose.getY()); +// double thetaOutput = 0.15 * RotationsPerSecond.of(0.75).in(RadiansPerSecond) * thetaController.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + +// if (DriverStation.getAlliance().get() == Alliance.Blue){ +// CommandSwerveDrivetrain.getInstance().applyRequest(() -> +// robotCentricRequest.withVelocityX(-XOutput) +// .withVelocityY(-YOutput) +// .withRotationalRate(thetaOutput) - ).execute(); - } else { - CommandSwerveDrivetrain.getInstance().applyRequest(() -> - robotCentricRequest.withVelocityX(XOutput) - .withVelocityY(YOutput) - .withRotationalRate(-thetaOutput) - ).execute(); - } - commandRan = true; - SmartDashboard.putString("Target Pose X", targetPose.toString()); - - - } +// ).execute(); +// } else { +// CommandSwerveDrivetrain.getInstance().applyRequest(() -> +// robotCentricRequest.withVelocityX(XOutput) +// .withVelocityY(YOutput) +// .withRotationalRate(-thetaOutput) +// ).execute(); +// } +// commandRan = true; +// SmartDashboard.putString("Target Pose X", targetPose.toString()); + + +// } - @Override - public boolean isFinished() { - commandRan = false; - return xDistanceController.atSetpoint() && yDistanceController.atSetpoint() && thetaController.atSetpoint(); - } +// @Override +// public boolean isFinished() { +// commandRan = false; +// return xDistanceController.atSetpoint() && yDistanceController.atSetpoint() && thetaController.atSetpoint(); +// } - @Override - public void end(boolean interrupted) { +// @Override +// public void end(boolean interrupted) { - } - - private Pose2d getTargetReefPose(boolean left){ - ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; - double X = currentPose.getTranslation().getX() * currentSpeeds.vxMetersPerSecond * 0.1; - double Y = currentPose.getTranslation().getY() * currentSpeeds.vyMetersPerSecond * 0.1; - - double intercept = Math.tan(Units.degreesToRadians(30))*4.5; - double interceptRed = Math.tan(Units.degreesToRadians(30))*13; - double slope = Math.tan(Units.degreesToRadians(30)); - - var currAlliance = DriverStation.getAlliance(); - - //TODO: map currentPose to a Reef Zone and then finding closest lineup based on heading - //joey... what the actual god-loving heck is this - - if(currAlliance.get() == Alliance.Blue) { - - if (Y <= -X*slope + intercept+4 && Y >= X*slope - intercept+4 && left && X <= 4.5) { - System.out.println("Ablue"); - return VisionConfig.AlignmentConfig.Ablue; - } else if (Y <= -X*slope + intercept+4 && Y >= X*slope - intercept+4 && !left && X <= 4.5) { - System.out.println("Bblue"); - return VisionConfig.AlignmentConfig.Bblue; - } else if (Y <= X*slope - intercept+4 && X <= 4.5 && left) { - System.out.println("Cblue"); - return VisionConfig.AlignmentConfig.Cblue; - } else if (Y <= X*slope - intercept+4 && X <= 4.5 && !left) { - System.out.println("Dblue"); - return VisionConfig.AlignmentConfig.Dblue; - } else if (Y <= -X*slope + intercept+4 && X >= 4.5 && !left) { - System.out.println("Eblue"); - return VisionConfig.AlignmentConfig.Eblue; - } else if (Y <= -X*slope + intercept+4 && X >= 4.5 && left) { - System.out.println("Fblue"); - return VisionConfig.AlignmentConfig.Fblue; - } else if (Y >= -X*slope + intercept+4 && Y <= X*slope - intercept+4 && !left && X >= 4.5) { - System.out.println("Gblue"); - return VisionConfig.AlignmentConfig.Gblue; - } else if (Y >= -X*slope + intercept+4 && Y <= X*slope - intercept+4 && left && X >= 4.5) { - System.out.println("Hblue"); - return VisionConfig.AlignmentConfig.Hblue; - } else if (Y >= X*slope - intercept+4 && X >= 4.5 && !left) { - System.out.println("Iblue"); - return VisionConfig.AlignmentConfig.Iblue; - } else if (Y >= X*slope - intercept+4 && X >= 4.5 && left) { - System.out.println("Jblue"); - return VisionConfig.AlignmentConfig.Jblue; - } else if (Y >= -X*slope + intercept+4 && X <= 4.5 && left) { - System.out.println("Kblue"); - return VisionConfig.AlignmentConfig.Kblue; - } else if (Y >= -X*slope + intercept+4 && X <= 4.5 && !left) { - System.out.println("Lblue"); - return VisionConfig.AlignmentConfig.Lblue; - } else { - System.out.println("error"); - return VisionConfig.AlignmentConfig.Error; - } - } - else if(currAlliance.get() == Alliance.Red) { - if (Y <= -X*slope + interceptRed+4 && Y >= X*slope - interceptRed+4 && !left && X <= 13.0) { - System.out.println("Gred"); - return VisionConfig.AlignmentConfig.Gred; - } else if (Y <= -X*slope + interceptRed+4 && Y >= X*slope - interceptRed+4 && left && X <= 13.0) { - System.out.println("Hred"); - return VisionConfig.AlignmentConfig.Hred; - } else if (Y <= X*slope - interceptRed+4 && X <= 13.0 && !left) { - System.out.println("Ired"); - return VisionConfig.AlignmentConfig.Ired; - } else if (Y <= X*slope - interceptRed+4 && X <= 13.0 && left) { - System.out.println("Jred"); - return VisionConfig.AlignmentConfig.Jred; - } else if (Y <= -X*slope + interceptRed+4 && X >= 13.0 && left) { - System.out.println("Kred"); - return VisionConfig.AlignmentConfig.Kred; - } else if (Y <= -X*slope + interceptRed+4 && X >= 13.0 && !left) { - System.out.println("Lred"); - return VisionConfig.AlignmentConfig.Lred; - } else if (Y >= -X*slope + interceptRed+4 && Y <= X*slope - interceptRed+4 && left && X >= 13.0) { - System.out.println("Ared"); - return VisionConfig.AlignmentConfig.Ared; - } else if (Y >= -X*slope + interceptRed+4 && Y <= X*slope - interceptRed+4 && !left && X >= 13.0) { - System.out.println("Bred"); - return VisionConfig.AlignmentConfig.Bred; - } else if (Y >= X*slope - interceptRed+4 && X >= 13.0 && left) { - System.out.println("Cred"); - return VisionConfig.AlignmentConfig.Cred; - } else if (Y >= X*slope - interceptRed+4 && X >= 13.0 && !left) { - System.out.println("Dred"); - return VisionConfig.AlignmentConfig.Dred; - } else if (Y >= -X*slope + interceptRed+4 && X <= 13.0 && !left) { - System.out.println("Ered"); - return VisionConfig.AlignmentConfig.Ered; - } else if (Y >= -X*slope + interceptRed+4 && X <= 13.0 && left) { - System.out.println("Fred"); - return VisionConfig.AlignmentConfig.Fred; - } else { - System.out.println("error"); - return VisionConfig.AlignmentConfig.Error; - } - } - else { - System.out.println("Error: Invalid Alliance (Lineup Command)"); - } - - System.out.println("Error: no suitable target pose found (Lineup Command)"); - return new Pose2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); - } -} +// } + +// private Pose2d getTargetReefPose(boolean left){ +// ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; +// double X = currentPose.getTranslation().getX() * currentSpeeds.vxMetersPerSecond * 0.1; +// double Y = currentPose.getTranslation().getY() * currentSpeeds.vyMetersPerSecond * 0.1; + +// double intercept = Math.tan(Units.degreesToRadians(30))*4.5; +// double interceptRed = Math.tan(Units.degreesToRadians(30))*13; +// double slope = Math.tan(Units.degreesToRadians(30)); + +// var currAlliance = DriverStation.getAlliance(); + +// //TODO: map currentPose to a Reef Zone and then finding closest lineup based on heading +// //joey... what the actual god-loving heck is this + +// if(currAlliance.get() == Alliance.Blue) { + +// if (Y <= -X*slope + intercept+4 && Y >= X*slope - intercept+4 && left && X <= 4.5) { +// System.out.println("Ablue"); +// return VisionConfig.AlignmentConfig.Ablue; +// } else if (Y <= -X*slope + intercept+4 && Y >= X*slope - intercept+4 && !left && X <= 4.5) { +// System.out.println("Bblue"); +// return VisionConfig.AlignmentConfig.Bblue; +// } else if (Y <= X*slope - intercept+4 && X <= 4.5 && left) { +// System.out.println("Cblue"); +// return VisionConfig.AlignmentConfig.Cblue; +// } else if (Y <= X*slope - intercept+4 && X <= 4.5 && !left) { +// System.out.println("Dblue"); +// return VisionConfig.AlignmentConfig.Dblue; +// } else if (Y <= -X*slope + intercept+4 && X >= 4.5 && !left) { +// System.out.println("Eblue"); +// return VisionConfig.AlignmentConfig.Eblue; +// } else if (Y <= -X*slope + intercept+4 && X >= 4.5 && left) { +// System.out.println("Fblue"); +// return VisionConfig.AlignmentConfig.Fblue; +// } else if (Y >= -X*slope + intercept+4 && Y <= X*slope - intercept+4 && !left && X >= 4.5) { +// System.out.println("Gblue"); +// return VisionConfig.AlignmentConfig.Gblue; +// } else if (Y >= -X*slope + intercept+4 && Y <= X*slope - intercept+4 && left && X >= 4.5) { +// System.out.println("Hblue"); +// return VisionConfig.AlignmentConfig.Hblue; +// } else if (Y >= X*slope - intercept+4 && X >= 4.5 && !left) { +// System.out.println("Iblue"); +// return VisionConfig.AlignmentConfig.Iblue; +// } else if (Y >= X*slope - intercept+4 && X >= 4.5 && left) { +// System.out.println("Jblue"); +// return VisionConfig.AlignmentConfig.Jblue; +// } else if (Y >= -X*slope + intercept+4 && X <= 4.5 && left) { +// System.out.println("Kblue"); +// return VisionConfig.AlignmentConfig.Kblue; +// } else if (Y >= -X*slope + intercept+4 && X <= 4.5 && !left) { +// System.out.println("Lblue"); +// return VisionConfig.AlignmentConfig.Lblue; +// } else { +// System.out.println("error"); +// return VisionConfig.AlignmentConfig.Error; +// } +// } +// else if(currAlliance.get() == Alliance.Red) { +// if (Y <= -X*slope + interceptRed+4 && Y >= X*slope - interceptRed+4 && !left && X <= 13.0) { +// System.out.println("Gred"); +// return VisionConfig.AlignmentConfig.Gred; +// } else if (Y <= -X*slope + interceptRed+4 && Y >= X*slope - interceptRed+4 && left && X <= 13.0) { +// System.out.println("Hred"); +// return VisionConfig.AlignmentConfig.Hred; +// } else if (Y <= X*slope - interceptRed+4 && X <= 13.0 && !left) { +// System.out.println("Ired"); +// return VisionConfig.AlignmentConfig.Ired; +// } else if (Y <= X*slope - interceptRed+4 && X <= 13.0 && left) { +// System.out.println("Jred"); +// return VisionConfig.AlignmentConfig.Jred; +// } else if (Y <= -X*slope + interceptRed+4 && X >= 13.0 && left) { +// System.out.println("Kred"); +// return VisionConfig.AlignmentConfig.Kred; +// } else if (Y <= -X*slope + interceptRed+4 && X >= 13.0 && !left) { +// System.out.println("Lred"); +// return VisionConfig.AlignmentConfig.Lred; +// } else if (Y >= -X*slope + interceptRed+4 && Y <= X*slope - interceptRed+4 && left && X >= 13.0) { +// System.out.println("Ared"); +// return VisionConfig.AlignmentConfig.Ared; +// } else if (Y >= -X*slope + interceptRed+4 && Y <= X*slope - interceptRed+4 && !left && X >= 13.0) { +// System.out.println("Bred"); +// return VisionConfig.AlignmentConfig.Bred; +// } else if (Y >= X*slope - interceptRed+4 && X >= 13.0 && left) { +// System.out.println("Cred"); +// return VisionConfig.AlignmentConfig.Cred; +// } else if (Y >= X*slope - interceptRed+4 && X >= 13.0 && !left) { +// System.out.println("Dred"); +// return VisionConfig.AlignmentConfig.Dred; +// } else if (Y >= -X*slope + interceptRed+4 && X <= 13.0 && !left) { +// System.out.println("Ered"); +// return VisionConfig.AlignmentConfig.Ered; +// } else if (Y >= -X*slope + interceptRed+4 && X <= 13.0 && left) { +// System.out.println("Fred"); +// return VisionConfig.AlignmentConfig.Fred; +// } else { +// System.out.println("error"); +// return VisionConfig.AlignmentConfig.Error; +// } +// } +// else { +// System.out.println("Error: Invalid Alliance (Lineup Command)"); +// } + +// System.out.println("Error: no suitable target pose found (Lineup Command)"); +// return new Pose2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); +// } +// } diff --git a/src/main/java/frc/robot/vision/commands/VisionCommands.java b/src/main/java/frc/robot/vision/commands/VisionCommands.java deleted file mode 100644 index b6e1f77..0000000 --- a/src/main/java/frc/robot/vision/commands/VisionCommands.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.vision.commands; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; - -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; - -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.estimator.PoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.vision.PoseEstimatorSubsystem; -import frc.robot.vision.VisionConfig; - -public class VisionCommands { - - public Command executeLineup() { - Pose2d estimatedPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); - - //add ProfiledPID controllers for Translation and Rotation - - - return new WaitCommand(0); - } - - - -} diff --git a/src/main/java/frc/robot/vision/old vision/PhotonRunnable.java b/src/main/java/frc/robot/vision/old vision/PhotonRunnable.java new file mode 100644 index 0000000..859da72 --- /dev/null +++ b/src/main/java/frc/robot/vision/old vision/PhotonRunnable.java @@ -0,0 +1,83 @@ +// package frc.robot.vision; + +// import static frc.robot.vision.VisionConfig.AMBIGUITY_THRESHOLD; + +// import java.io.IOException; +// import java.util.concurrent.atomic.AtomicReference; + +// import org.photonvision.EstimatedRobotPose; +// import org.photonvision.PhotonCamera; +// import org.photonvision.PhotonPoseEstimator; +// import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +// import edu.wpi.first.apriltag.AprilTagFieldLayout; +// import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +// import edu.wpi.first.apriltag.AprilTagFields; +// import edu.wpi.first.math.geometry.Transform3d; +// import edu.wpi.first.wpilibj.DriverStation; + +// /** +// * Runnable that gets AprilTag data from PhotonVision. +// */ +// public class PhotonRunnable implements Runnable { + +// private final PhotonPoseEstimator photonPoseEstimator; +// private final PhotonCamera photonCamera; +// private final AtomicReference atomicEstimatedRobotPose = new AtomicReference(); + +// public PhotonRunnable(PhotonCamera cameraName, Transform3d robotToCamera) { +// this.photonCamera = cameraName; +// PhotonPoseEstimator photonPoseEstimator = null; +// try { +// var layout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); +// // PV estimates will always be blue, they'll get flipped by robot thread +// layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); +// if (photonCamera != null) { +// photonPoseEstimator = new PhotonPoseEstimator( +// layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); +// photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); +// } +// } catch (Exception e) { +// DriverStation.reportError("Try Catch Block Fail in PhotonRunnable.java", e.getStackTrace()); +// photonPoseEstimator = null; +// } +// this.photonPoseEstimator = photonPoseEstimator; +// } + +// @Override +// public void run() { +// // Get AprilTag data +// if (photonPoseEstimator != null && photonCamera != null) { +// var photonResults = photonCamera.getAllUnreadResults(); +// for(var result : photonResults) { +// if (result.hasTargets() +// && (result.targets.size() > 1 +// || result.targets.get(0).getPoseAmbiguity() < AMBIGUITY_THRESHOLD)) { +// photonPoseEstimator.update(result).ifPresent(estimatedRobotPose -> { +// var estimatedPose = estimatedRobotPose.estimatedPose; +// // Make sure the measurement is on the field +// if (estimatedPose.getX() > 0.0 && estimatedPose.getX() <= VisionConfig.FIELD_LENGTH_METERS +// && estimatedPose.getY() > 0.0 && estimatedPose.getY() <= VisionConfig.FIELD_WIDTH_METERS) { +// atomicEstimatedRobotPose.set(estimatedRobotPose); +// } +// }); +// } +// } + +// } +// } + +// /** +// * Gets the latest robot pose. Calling this will only return the pose once. If +// * it returns a non-null value, it is a +// * new estimate that hasn't been returned before. +// * This pose will always be for the BLUE alliance. It must be flipped if the +// * current alliance is RED. +// * +// * @return latest estimated pose +// */ +// public EstimatedRobotPose grabLatestEstimatedPose() { +// return atomicEstimatedRobotPose.getAndSet(null); +// } + +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/old vision/PoseEstimatorSubsystem.java b/src/main/java/frc/robot/vision/old vision/PoseEstimatorSubsystem.java new file mode 100644 index 0000000..8546771 --- /dev/null +++ b/src/main/java/frc/robot/vision/old vision/PoseEstimatorSubsystem.java @@ -0,0 +1,254 @@ +// package frc.robot.vision; + +// import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide; +// import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide; + +// import java.util.function.Supplier; + +// import org.photonvision.EstimatedRobotPose; +// import org.photonvision.PhotonCamera; + +// import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.SwerveModulePosition; +// import edu.wpi.first.math.numbers.N1; +// import edu.wpi.first.math.numbers.N3; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.Notifier; +// import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +// import edu.wpi.first.wpilibj.smartdashboard.Field2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.robot.drivetrain.CommandSwerveDrivetrain; + +// public class PoseEstimatorSubsystem extends SubsystemBase { + +// // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much +// // you trust your various sensors. Smaller numbers will cause the filter to +// // "trust" the estimate from that particular component more than the others. +// // This in turn means the particualr component will have a stronger influence +// // on the final pose estimate. + +// // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much +// // you trust your various sensors. Smaller numbers will cause the filter to +// // "trust" the estimate from that particular component more than the others. +// // This in turn means the particualr component will have a stronger influence +// // on the final pose estimate. + +// private final Supplier rotationSupplier; +// private final Supplier modulePositionSupplier; +// private final SwerveDrivePoseEstimator poseEstimator; +// private final Field2d field2d = new Field2d(); +// private final PhotonRunnable rightEstimator = new PhotonRunnable(new PhotonCamera(VisionConfig.CAM_NAMES[1]), +// VisionConfig.ROBOT_TO_CAM_TRANSFORMS[1]); +// private final PhotonRunnable leftEstimator = new PhotonRunnable(new PhotonCamera(VisionConfig.CAM_NAMES[0]), +// VisionConfig.ROBOT_TO_CAM_TRANSFORMS[0]); + +// private final Notifier allNotifier = new Notifier(() -> { +// rightEstimator.run(); +// leftEstimator.run(); +// }); + +// private OriginPosition originPosition = kBlueAllianceWallRightSide; + +// private static PoseEstimatorSubsystem mInstance; + +// public PoseEstimatorSubsystem(Supplier rotationSupplier, +// Supplier modulePositionSupplier) { +// this.rotationSupplier = rotationSupplier; +// this.modulePositionSupplier = modulePositionSupplier; + +// poseEstimator = new SwerveDrivePoseEstimator( +// CommandSwerveDrivetrain.getInstance().getKinematics(), +// rotationSupplier.get(), +// modulePositionSupplier.get(), +// new Pose2d(), +// VisionConfig.STATE_STANDARD_DEVIATIONS, +// VisionConfig.VISION_MEASUREMENT_STANDARD_DEVIATIONS); + +// // Start PhotonVision thread +// // rightNotifier.setName("rightRunnable"); +// // rightNotifier.startPeriodic(0.02); + +// // // Start PhotonVision thread +// // leftNotifier.setName("leftRunnable"); +// // leftNotifier.startPeriodic(0.02); + +// allNotifier.setName("runAll"); +// allNotifier.startPeriodic(0.02); + +// SmartDashboard.putData("Field Pose Estimation", field2d); +// // backNotifier.setName("backRunnable"); +// // backNotifier.startPeriodic(0.02); + +// } + +// public static PoseEstimatorSubsystem getInstance() { +// if(mInstance == null) { +// CommandSwerveDrivetrain mDrivetrain = CommandSwerveDrivetrain.getInstance(); +// mInstance = new PoseEstimatorSubsystem(mDrivetrain::getGyroRotation, mDrivetrain::getSwerveModulePositions); +// } +// return mInstance; +// } + +// public void addDashboardWidgets(ShuffleboardTab tab) { +// tab.add("Field", field2d).withPosition(0, 0).withSize(6, 4); +// tab.addString("Pose", this::getFomattedPose).withPosition(6, 2).withSize(2, 1); +// } + +// /** +// * Sets the alliance. This is used to configure the origin of the AprilTag map +// * +// * @param alliance alliance +// */ +// public void setAlliance(Alliance alliance) { +// boolean allianceChanged = false; +// switch (alliance) { +// case Blue: +// allianceChanged = (originPosition == kRedAllianceWallRightSide); +// originPosition = kBlueAllianceWallRightSide; +// break; +// case Red: +// allianceChanged = (originPosition == kBlueAllianceWallRightSide); +// originPosition = kRedAllianceWallRightSide; +// break; +// default: +// // No valid alliance data. Nothing we can do about it +// } + +// if (allianceChanged) { +// // The alliance changed, which changes the coordinate system. +// // Since a tag was seen, and the tags are all relative to the coordinate system, +// // the estimated pose +// // needs to be transformed to the new coordinate system. +// var newPose = flipAlliance(getCurrentPose()); +// poseEstimator.resetPosition(rotationSupplier.get(), modulePositionSupplier.get(), newPose); +// } +// } + +// @Override +// public void periodic() { +// // Update pose estimator with drivetrain sensors +// poseEstimator.update(rotationSupplier.get(), modulePositionSupplier.get()); +// if (VisionConfig.USE_VISION) { +// estimatorChecker(rightEstimator); +// estimatorChecker(leftEstimator); +// } else { +// allNotifier.close(); +// } + +// // estimatorChecker(backEstimator); + +// // Set the pose on the dashboard +// var dashboardPose = poseEstimator.getEstimatedPosition(); +// if (originPosition == kRedAllianceWallRightSide) { +// // Flip the pose when red, since the dashboard field photo cannot be rotated +// dashboardPose = flipAlliance(dashboardPose); +// } +// field2d.setRobotPose(dashboardPose); +// SmartDashboard.putString("Pose Formatted", getFomattedPose()); + +// } + +// private String getFomattedPose() { +// var pose = getCurrentPose(); +// return String.format("(%.3f, %.3f) %.2f degrees", +// pose.getX(), +// pose.getY(), +// pose.getRotation().getDegrees()); +// } + +// public Pose2d getCurrentPose() { +// return poseEstimator.getEstimatedPosition(); +// } + +// /** +// * Resets the current pose to the specified pose. This should ONLY be called +// * when the robot's position on the field is known, like at the beginning of +// * a match. +// * +// * @param newPose new pose +// */ +// public void setCurrentPose(Pose2d newPose) { +// poseEstimator.resetPosition(rotationSupplier.get(), modulePositionSupplier.get(), newPose); +// } + +// /** +// * Resets the position on the field to 0,0 0-degrees, with forward being +// * downfield. This resets +// * what "forward" is for field oriented driving. +// */ +// public void resetFieldPosition() { +// setCurrentPose(new Pose2d()); +// } + +// /** +// * Transforms a pose to the opposite alliance's coordinate system. (0,0) is +// * always on the right corner of your +// * alliance wall, so for 2023, the field elements are at different coordinates +// * for each alliance. +// * +// * @param poseToFlip pose to transform to the other alliance +// * @return pose relative to the other alliance's coordinate system +// */ +// private Pose2d flipAlliance(Pose2d poseToFlip) { +// return poseToFlip.relativeTo(new Pose2d( +// new Translation2d(VisionConfig.FIELD_LENGTH_METERS, VisionConfig.FIELD_WIDTH_METERS), +// new Rotation2d(Math.PI))); +// } + +// public void addTrajectory(Trajectory traj) { +// field2d.getObject("Trajectory").setTrajectory(traj); +// } + +// // public void resetPoseRating() { +// // xValues.clear(); +// // yValues.clear(); +// // } + +// private Matrix confidenceCalculator(EstimatedRobotPose estimation) { +// double smallestDistance = Double.POSITIVE_INFINITY; +// for (var target : estimation.targetsUsed) { +// var t3d = target.getBestCameraToTarget(); +// var distance = Math.sqrt(Math.pow(t3d.getX(), 2) + Math.pow(t3d.getY(), 2) + Math.pow(t3d.getZ(), 2)); +// if (distance < smallestDistance) +// smallestDistance = distance; +// } +// double poseAmbiguityFactor = estimation.targetsUsed.size() != 1 +// ? 1 +// : Math.max( +// 1, +// (estimation.targetsUsed.get(0).getPoseAmbiguity() +// + VisionConfig.POSE_AMBIGUITY_SHIFTER) +// * VisionConfig.POSE_AMBIGUITY_MULTIPLIER); +// double confidenceMultiplier = Math.max( +// 1, +// (Math.max( +// 1, +// Math.max(0, smallestDistance - VisionConfig.NOISY_DISTANCE_METERS) +// * VisionConfig.DISTANCE_WEIGHT) +// * poseAmbiguityFactor) +// / (1 +// + ((estimation.targetsUsed.size() - 1) * VisionConfig.TAG_PRESENCE_WEIGHT))); + +// return VisionConfig.VISION_MEASUREMENT_STANDARD_DEVIATIONS.times(confidenceMultiplier); +// } + +// public void estimatorChecker(PhotonRunnable estimator) { +// var cameraPose = estimator.grabLatestEstimatedPose(); +// if (cameraPose != null) { +// // New pose from vision +// var pose2d = cameraPose.estimatedPose.toPose2d(); +// if (originPosition == kRedAllianceWallRightSide) { +// pose2d = flipAlliance(pose2d); +// } +// poseEstimator.addVisionMeasurement(pose2d, cameraPose.timestampSeconds, +// confidenceCalculator(cameraPose)); +// } +// } +// } \ No newline at end of file