From 5f9a38f55d5774614469df958ae92836bd06060a Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:32:13 -0700 Subject: [PATCH 1/5] 2024x2025 --- src/main/java/frc4388/robot/Constants.java | 9 +++++++++ src/main/java/frc4388/robot/RobotContainer.java | 12 ++++++------ .../java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- src/main/java/frc4388/robot/subsystems/Vision.java | 6 +++++- 4 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0108060..f036098 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -99,40 +99,49 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + // public static final double FORWARD_OFFSET = 270; // 0, 90, 180, 270 public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); private static final class ModuleSpecificConstants { //Front Left + // private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + // private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right + // private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + // private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left + // private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + // private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right + // private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + // private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b127f61..9aa79d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -155,14 +155,14 @@ public class RobotContainer { // @ /* Trim Test Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 14bf6a8..2ae0061 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -181,7 +181,7 @@ public class SwerveDrive extends Subsystem { } public void stopModules() { - // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 07ca483..d121854 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; @@ -73,6 +74,9 @@ public class Vision extends Subsystem { @Override public void periodic() { var result = camera.getLatestResult(); + // var results = camera.getAllUnreadResults(); + // if (results.size() == 0) return; + // var result = results.get(results.size()-1); isTag = result.hasTargets(); // Optional multitag = result.getMultiTagResult(); @@ -106,7 +110,7 @@ public class Vision extends Subsystem { } lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); + lastVisionPose.rotateBy(Rotation2d.k180deg); // lastVisionPose = new Pose2d( // lastVisionPose.getTranslation(), // lastPhysOdomPose.getRotation() From 063fb74b7814551ff5331d921b6bda85585513dc Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 15 Jan 2025 17:55:24 -0700 Subject: [PATCH 2/5] Fix 2024 auto align --- src/main/java/frc4388/robot/Constants.java | 36 +++++++++---------- .../java/frc4388/robot/RobotContainer.java | 3 +- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../java/frc4388/robot/subsystems/Vision.java | 2 +- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f036098..8f7e30e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -99,50 +99,50 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - // public static final double FORWARD_OFFSET = 270; // 0, 90, 180, 270 - public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 + // public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); private static final class ModuleSpecificConstants { //Front Left - // private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); + // private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - // private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + // private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - // private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); + // private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - // private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + // private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left - // private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); + // private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - // private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); - private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + // private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right - // private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); + // private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - // private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); + // private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9aa79d9..bc03221 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -89,6 +89,7 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + configureButtonBindings(); configureVirtualButtonBindings(); new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); @@ -101,7 +102,7 @@ public class RobotContainer { m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), - false); + true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2ae0061..bd96aff 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -79,7 +79,7 @@ public class SwerveDrive extends Subsystem { if (fieldRelative) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityX(leftStick.getX()*-speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index d121854..a32b0cd 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -110,7 +110,7 @@ public class Vision extends Subsystem { } lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - lastVisionPose.rotateBy(Rotation2d.k180deg); + // lastVisionPose.rotateBy(Rotation2d.k180deg); // lastVisionPose = new Pose2d( // lastVisionPose.getTranslation(), // lastPhysOdomPose.getRotation() From 56f3ccf0c56c231516fc6541c50d9991f3feb88c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 16 Jan 2025 16:51:09 -0700 Subject: [PATCH 3/5] Add Reef Position Helper --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 26 +++--- .../java/frc4388/robot/subsystems/Vision.java | 3 +- .../frc4388/utility/ReefPositionHelper.java | 88 +++++++++++++++++++ 4 files changed, 107 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc4388/utility/ReefPositionHelper.java diff --git a/build.gradle b/build.gradle index 302477e..689ff84 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8f7e30e..736af3a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,6 +46,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; @@ -301,16 +302,6 @@ public final class Constants { public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); - // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - - // Test april tag field layout - public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - Arrays.asList(new AprilTag[] { - new AprilTag(1, new Pose3d( - new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - )), - }), 100, 100); // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) @@ -318,6 +309,21 @@ public final class Constants { public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } + public static final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2025Reefscape.loadAprilTagLayoutField(); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(13); + + + // Test april tag field layout + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(1, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); + + } + public static final class DriveConstants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index a32b0cd..d88a2d4 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; @@ -67,7 +68,7 @@ public class Vision extends Subsystem { this.camera = camera; SmartDashboard.putData(field); - photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); + photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java new file mode 100644 index 0000000..15bbf7c --- /dev/null +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -0,0 +1,88 @@ +package frc4388.utility; + +import java.util.Optional; + +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc4388.robot.Constants.FieldConstants; + +public class ReefPositionHelper { + public enum Side { + LEFT, + RIGHT + } + + public static Pose2d[] RED_TAGS = { + FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(11).get().toPose2d() + }; + + public static Pose2d[] BLUE_TAGS = { + FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(22).get().toPose2d() + }; + + public static double distanceTo(Pose2d first, Pose2d second){ + return Math.sqrt( Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); + } + + + /* + * Function to loop through a list of tag locations to figure out closest one + */ + public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){ + if(locations.length <= 0) return new Pose2d(); + + Pose2d minPos = locations[0]; + double minDistance = distanceTo(position,minPos); + + for(int i = 1; i < locations.length; i++){ + double dist = distanceTo(locations[i],minPos); + if(dist < minDistance){ + minPos = locations[i]; + minDistance = dist; + } + } + + return minPos; + } + + /* + * Function to find closest tag location based on side + */ + public static Pose2d getNearestTag(Pose2d position) { + Optional ally = DriverStation.getAlliance(); + if (!ally.isPresent()) + return new Pose2d(); + if (ally.get() == Alliance.Red) + return getNearestTag(RED_TAGS, position); + if (ally.get() == Alliance.Blue) + return getNearestTag(BLUE_TAGS, position); + return new Pose2d(); + } + + public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) { + return getNearestTag(horisontalOffset(position, side == Side.LEFT ? -(xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET))); + } + + + public static Pose2d horisontalOffset(Pose2d oldPose, double xoffset){ + Translation2d oldTranslation = oldPose.getTranslation(); + Rotation2d rot = oldPose.getRotation(); + return new Pose2d(new Translation2d( + oldTranslation.getX() + rot.getCos() * xoffset, + oldTranslation.getY() + rot.getSin() * xoffset + ), oldPose.getRotation()); + } +} From 9db9d25f5ba3bbd5d58db98b3ca2d65a9487bf6b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 16 Jan 2025 19:41:05 -0700 Subject: [PATCH 4/5] Add trims and get reef position --- .vscode/settings.json | 3 +- src/main/java/frc4388/robot/Constants.java | 12 +++++-- .../java/frc4388/robot/RobotContainer.java | 15 +++++++-- .../robot/commands/GotoPositionCommand.java | 6 ++-- .../java/frc4388/robot/subsystems/Vision.java | 31 ++++++++++++------- .../frc4388/utility/ReefPositionHelper.java | 25 +++++++++------ 6 files changed, 64 insertions(+), 28 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c..d2f7034 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 736af3a..68913be 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -52,6 +52,7 @@ import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.ReefPositionHelper; import frc4388.utility.Trim; /** @@ -103,7 +104,7 @@ public final class Constants { public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 // public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 - public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); + // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); private static final class ModuleSpecificConstants { //Front Left @@ -199,11 +200,14 @@ public final class Constants { public static final Gains XY_GAINS = new Gains(3,0,0); public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0); + public static final double XY_TOLERANCE = 0.05; public static final double ROT_TOLERANCE = 1; - public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); + // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); + // public static final Pose2d targetpos = } @@ -311,7 +315,9 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2025Reefscape.loadAprilTagLayoutField(); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(13); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); + // Test april tag field layout diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bc03221..f0e76b6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,7 +42,9 @@ import frc4388.robot.subsystems.SwerveDrive; // Utilites import frc4388.utility.DeferredBlock; +import frc4388.utility.ReefPositionHelper; import frc4388.utility.Subsystem; +import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.configurable.ConfigurableString; /** @@ -165,6 +167,15 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + + + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -181,7 +192,7 @@ public class RobotContainer { // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos)); + .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); @@ -237,7 +248,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { //return autoPlayback; - return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); + return new GotoPositionCommand(m_robotSwerveDrive, m_vision); } /** diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index e7d82ed..1d5755b 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -9,6 +9,8 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; import frc4388.utility.Gains; +import frc4388.utility.ReefPositionHelper; +import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.VirtualController; @@ -32,10 +34,9 @@ public class GotoPositionCommand extends Command { * @param SwerveDrive m_robotSwerveDrive */ - public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { + public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; - this.targetpos = targetpos; addRequirements(swerveDrive); } @@ -43,6 +44,7 @@ public class GotoPositionCommand extends Command { public void initialize() { xPID.initialize(); yPID.initialize(); + this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get()); } double xerr; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index d88a2d4..25807c9 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -40,7 +40,8 @@ public class Vision extends Subsystem { private PhotonCamera camera; - private boolean isTag = false; + private boolean isTagDetected = false; + private boolean isTagProcessed = false; private Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); @@ -54,10 +55,15 @@ public class Vision extends Subsystem { .getLayout(getSubsystemName(), BuiltInLayouts.kList) .withSize(2, 2); - GenericEntry sbTag = subsystemLayout + GenericEntry sbTagDetected = subsystemLayout .add("Tag Detected", false) .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); + + GenericEntry sbTagProcessed = subsystemLayout + .add("Tag Processed", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); GenericEntry sbCamConnected = subsystemLayout .add("Camera Connnected", false) @@ -78,7 +84,8 @@ public class Vision extends Subsystem { // var results = camera.getAllUnreadResults(); // if (results.size() == 0) return; // var result = results.get(results.size()-1); - isTag = result.hasTargets(); + isTagDetected = result.hasTargets(); + isTagProcessed = false; // Optional multitag = result.getMultiTagResult(); @@ -94,8 +101,8 @@ public class Vision extends Subsystem { // System.out.println(isTag); - if(!isTag){ - sbTag.setBoolean(isTag); + if(!isTagDetected){ + // sbTagDetected.setBoolean(isTagDetected); field.setRobotPose(getPose2d()); return; } @@ -104,12 +111,13 @@ public class Vision extends Subsystem { // In case the pose estimator fails to estimate the pose, fallback to physical odometry. if(EstimatedRobotPose.isEmpty()){ - isTag = false; - sbTag.setBoolean(isTag); - field.setRobotPose(getPose2d()); + // sbTagProcessed.setBoolean(isTagProcessed); + field.setRobotPose(lastVisionPose); return; } + isTagProcessed = true; + lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); // lastVisionPose.rotateBy(Rotation2d.k180deg); // lastVisionPose = new Pose2d( @@ -234,7 +242,7 @@ public class Vision extends Subsystem { } public Pose2d getPose2d() { - if(isTag) + if(isTagDetected && isTagProcessed) return lastVisionPose; else return lastPhysOdomPose; @@ -245,7 +253,7 @@ public class Vision extends Subsystem { } public boolean isTag(){ - return isTag; + return isTagDetected && isTagProcessed; } @@ -271,7 +279,8 @@ public class Vision extends Subsystem { @Override public void queryStatus() { - sbTag.setBoolean(isTag); + sbTagDetected.setBoolean(isTagDetected); + sbTagProcessed.setBoolean(isTagProcessed); sbCamConnected.setBoolean(camera.isConnected()); // field.setRobotPose(getPose2d()); } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index 15bbf7c..47a7991 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -15,7 +15,7 @@ public class ReefPositionHelper { RIGHT } - public static Pose2d[] RED_TAGS = { + public static final Pose2d[] RED_TAGS = { FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(), FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(), FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(), @@ -24,7 +24,7 @@ public class ReefPositionHelper { FieldConstants.kTagLayout.getTagPose(11).get().toPose2d() }; - public static Pose2d[] BLUE_TAGS = { + public static final Pose2d[] BLUE_TAGS = { FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(), FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(), FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(), @@ -34,7 +34,7 @@ public class ReefPositionHelper { }; public static double distanceTo(Pose2d first, Pose2d second){ - return Math.sqrt( Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); + return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); } @@ -48,13 +48,16 @@ public class ReefPositionHelper { double minDistance = distanceTo(position,minPos); for(int i = 1; i < locations.length; i++){ - double dist = distanceTo(locations[i],minPos); + double dist = distanceTo(locations[i],position); if(dist < minDistance){ + System.out.println(i); minPos = locations[i]; minDistance = dist; } } + System.out.println(minPos); + return minPos; } @@ -73,16 +76,20 @@ public class ReefPositionHelper { } public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) { - return getNearestTag(horisontalOffset(position, side == Side.LEFT ? -(xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET))); + return offset(getNearestTag(position), + (side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim, + FieldConstants.VERTICAL_SCORING_POSITION_OFFSET); } - public static Pose2d horisontalOffset(Pose2d oldPose, double xoffset){ + public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){ Translation2d oldTranslation = oldPose.getTranslation(); - Rotation2d rot = oldPose.getRotation(); + + double rot = oldPose.getRotation().getRadians(); + return new Pose2d(new Translation2d( - oldTranslation.getX() + rot.getCos() * xoffset, - oldTranslation.getY() + rot.getSin() * xoffset + oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset, + oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset ), oldPose.getRotation()); } } From d702d30e0a9986df61888c08f5d509bf43ba8c96 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 17 Jan 2025 19:34:08 -0700 Subject: [PATCH 5/5] Add vision minimum --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/subsystems/Vision.java | 70 +++++++++---------- 2 files changed, 37 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 68913be..c45c4ce 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -306,7 +306,9 @@ public final class Constants { public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters + // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 25807c9..aeed160 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -20,6 +20,7 @@ import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.Utils; @@ -80,26 +81,14 @@ public class Vision extends Subsystem { @Override public void periodic() { - var result = camera.getLatestResult(); - // var results = camera.getAllUnreadResults(); - // if (results.size() == 0) return; - // var result = results.get(results.size()-1); + // var result = camera.getLatestResult(); + var results = camera.getAllUnreadResults(); + if (results.size() == 0) return; + var result = results.get(results.size()-1); + isTagDetected = result.hasTargets(); isTagProcessed = false; - // Optional multitag = result.getMultiTagResult(); - - // if (multitag.isEmpty()) { - // Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best; - // }else if() - - - // sbTag.setBoolean(isTag); - // field.setRobotPose(getPose2d()); - - // sbCamConnected.setBoolean(camera); - - // System.out.println(isTag); if(!isTagDetected){ // sbTagDetected.setBoolean(isTagDetected); @@ -107,12 +96,11 @@ public class Vision extends Subsystem { return; } - var EstimatedRobotPose = getEstimatedGlobalPose(); + var EstimatedRobotPose = getEstimatedGlobalPose(result); // In case the pose estimator fails to estimate the pose, fallback to physical odometry. if(EstimatedRobotPose.isEmpty()){ - // sbTagProcessed.setBoolean(isTagProcessed); - field.setRobotPose(lastVisionPose); + field.setRobotPose(getPose2d()); return; } @@ -143,9 +131,22 @@ public class Vision extends Subsystem { * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets * used for estimation. */ - public Optional getEstimatedGlobalPose() { + public Optional getEstimatedGlobalPose(PhotonPipelineResult change) { Optional visionEst = Optional.empty(); - for (var change : camera.getAllUnreadResults()) { + // for (var change : camera.getAllUnreadResults()) { + + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); + } + } + + if(targets.size() <= 0) + return visionEst; // Will be empty + visionEst = photonEstimator.update(change); updateEstimationStdDevs(visionEst, change.getTargets()); @@ -159,16 +160,11 @@ public class Vision extends Subsystem { // getSimDebugField().getObject("VisionEstimation").setPoses(); // }); // } - } + // } return visionEst; } - - - - - /** * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard * deviations based on number of tags, estimation strategy, and distance from the tags. @@ -192,13 +188,17 @@ public class Vision extends Subsystem { for (var tgt : targets) { var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; - numTags++; - avgDist += - tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + double distance = tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + numTags++; + avgDist += distance; + } } if (numTags == 0) {