Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #12 from Team4388/2024x2025

Browse Source 
2024x2025
This commit is contained in:
C4llSqin
2025-01-17 20:41:22 -07:00
committed by GitHub
parent e8fe7a1d52 383e88a3f8
commit 14818c32b6
8 changed files with 250 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+2 -1
View File
@@ -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"
}
build.gradle
+1 -1
View File
@@ -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 {
src/main/java/frc4388/robot/Constants.java
+76 -28
View File
@@ -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<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> 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;
}
src/main/java/frc4388/robot/RobotContainer.java
+6 -3
View File
@@ -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
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+4 -2
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+10 -10
View File
@@ -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() {
src/main/java/frc4388/robot/subsystems/Vision.java
+56 -42
View File
@@ -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<MultiTargetPNPResult> 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<EstimatedRobotPose> getEstimatedGlobalPose() {
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change) {
Optional<EstimatedRobotPose> 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());
}
src/main/java/frc4388/utility/ReefPositionHelper.java
+95
View File
@@ -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<Alliance> 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());
}
}