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/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 249a881..66f7cc7 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,11 +46,13 @@ 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; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.ReefPositionHelper; import frc4388.utility.Trim; /** @@ -101,42 +103,76 @@ public final class Constants { public static final double FORWARD_OFFSET = 90; // 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 { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = true; + 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_HEIGHT); - private static final Distance FRONT_LEFT_YPOS = 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.371337890625+.5); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; + 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_HEIGHT); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_WIDTH); + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = true; + 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_HEIGHT); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.07666015625); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = true; + 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_HEIGHT); - private static final Distance BACK_RIGHT_YPOS = 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); } + /* private static final class ModuleSpecificConstants { // 2024 + //Front Left + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); + 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_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); + 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); + + //Back Left + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.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); + + //Back Right + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); + 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_YPOS = Inches.of(-HALF_HEIGHT); + } */ + public static final class IDs { public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); @@ -189,11 +225,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 = } @@ -292,23 +331,32 @@ 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); + 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); 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(6.5); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); + + + + // 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ecbc21b..00689c6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -43,7 +43,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; /** @@ -90,6 +92,7 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + configureButtonBindings(); configureVirtualButtonBindings(); new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); @@ -153,7 +156,7 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -170,7 +173,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)); @@ -231,7 +234,7 @@ public class RobotContainer { // Load the path you want to follow using its name in the GUI return new PathPlannerAuto("New Auto"); } catch (Exception e) { - DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); return Commands.none(); } // zach told me to do the below comment 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/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 65db294..642c02d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -124,13 +124,13 @@ public class SwerveDrive extends Subsystem { leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - if (fieldRelative) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(-leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - // double rot = 0; + if (fieldRelative) { + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(-leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); + // double rot = 0; // ! drift correction // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. @@ -227,9 +227,9 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.tareEverything(); } - public void stopModules() { - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); - } + public void stopModules() { + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + } @Override public void periodic() { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 07ca483..aeed160 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; @@ -19,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; @@ -30,6 +32,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; @@ -38,7 +41,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(); @@ -52,10 +56,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) @@ -66,47 +75,39 @@ 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); } @Override public void periodic() { - var result = camera.getLatestResult(); - isTag = result.hasTargets(); + // 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(!isTag){ - sbTag.setBoolean(isTag); + if(!isTagDetected){ + // sbTagDetected.setBoolean(isTagDetected); field.setRobotPose(getPose2d()); 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()){ - isTag = false; - sbTag.setBoolean(isTag); field.setRobotPose(getPose2d()); return; } + isTagProcessed = true; + lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); + // lastVisionPose.rotateBy(Rotation2d.k180deg); // lastVisionPose = new Pose2d( // lastVisionPose.getTranslation(), // lastPhysOdomPose.getRotation() @@ -130,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()); @@ -146,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. @@ -179,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) { @@ -229,7 +242,7 @@ public class Vision extends Subsystem { } public Pose2d getPose2d() { - if(isTag) + if(isTagDetected && isTagProcessed) return lastVisionPose; else return lastPhysOdomPose; @@ -240,7 +253,7 @@ public class Vision extends Subsystem { } public boolean isTag(){ - return isTag; + return isTagDetected && isTagProcessed; } @@ -266,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 new file mode 100644 index 0000000..47a7991 --- /dev/null +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -0,0 +1,95 @@ +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 final 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 final 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],position); + if(dist < minDistance){ + System.out.println(i); + minPos = locations[i]; + minDistance = dist; + } + } + + System.out.println(minPos); + + 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 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 offset(Pose2d oldPose, double xoffset, double yoffset){ + Translation2d oldTranslation = oldPose.getTranslation(); + + double rot = oldPose.getRotation().getRadians(); + + return new Pose2d(new Translation2d( + 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()); + } +}