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] 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()); } }