Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add Reef Position Helper

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-16 16:51:09 -07:00
parent 063fb74b78
commit 56f3ccf0c5
4 changed files with 107 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+16 -10
View File
@@ -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<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(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;
}
src/main/java/frc4388/robot/subsystems/Vision.java
+2 -1
View File
@@ -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);
}