From a0fc780102705ea23fb76b08d8c9eb94a0bc1f49 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Thu, 3 Apr 2025 20:52:40 -0400 Subject: [PATCH] state changes --- .../deploy/pathplanner/autos/Mid_1_G.auto | 6 + src/main/deploy/pathplanner/paths/C_Load.path | 8 +- src/main/deploy/pathplanner/paths/D_Load.path | 8 +- src/main/deploy/pathplanner/paths/F_Load.path | 8 +- src/main/deploy/pathplanner/paths/I_Load.path | 8 +- src/main/deploy/pathplanner/paths/K_Load.path | 8 +- src/main/deploy/pathplanner/paths/L_Load.path | 10 +- src/main/deploy/pathplanner/paths/Load_C.path | 8 +- src/main/deploy/pathplanner/paths/Load_D.path | 8 +- src/main/deploy/pathplanner/paths/Load_K.path | 8 +- src/main/deploy/pathplanner/paths/Load_L.path | 8 +- .../java/frc/robot/vision/VisionConfig.java | 263 +----------------- .../frc/robot/vision/commands/AutoAlign.java | 6 +- 13 files changed, 51 insertions(+), 306 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Mid_1_G.auto b/src/main/deploy/pathplanner/autos/Mid_1_G.auto index 6d20cdc..4530010 100644 --- a/src/main/deploy/pathplanner/autos/Mid_1_G.auto +++ b/src/main/deploy/pathplanner/autos/Mid_1_G.auto @@ -25,6 +25,12 @@ "data": { "name": "LineUpRight" } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } } ] } diff --git a/src/main/deploy/pathplanner/paths/C_Load.path b/src/main/deploy/pathplanner/paths/C_Load.path index ba17e4b..8ceb549 100644 --- a/src/main/deploy/pathplanner/paths/C_Load.path +++ b/src/main/deploy/pathplanner/paths/C_Load.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6672500000000001, - "y": 0.6807499999999993 + "x": 1.693421052631579, + "y": 0.7151315789473688 }, "prevControl": { - "x": 2.091426034836473, - "y": 1.5330974051593276 + "x": 2.1175970874680523, + "y": 1.5674789841066974 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/D_Load.path b/src/main/deploy/pathplanner/paths/D_Load.path index 6a26872..f6343ee 100644 --- a/src/main/deploy/pathplanner/paths/D_Load.path +++ b/src/main/deploy/pathplanner/paths/D_Load.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6672500000000001, - "y": 0.6807499999999993 + "x": 1.693421052631579, + "y": 0.7151315789473688 }, "prevControl": { - "x": 2.1224648400109403, - "y": 0.9559681053027043 + "x": 2.148635892642519, + "y": 0.9903496842500739 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F_Load.path b/src/main/deploy/pathplanner/paths/F_Load.path index 0de6dd6..5dd934c 100644 --- a/src/main/deploy/pathplanner/paths/F_Load.path +++ b/src/main/deploy/pathplanner/paths/F_Load.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6672500000000001, - "y": 0.6807499999999993 + "x": 1.693421052631579, + "y": 0.7151315789473688 }, "prevControl": { - "x": 2.5403478260869576, - "y": 1.3337391304347817 + "x": 2.566518878718536, + "y": 1.3681207093821506 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/I_Load.path b/src/main/deploy/pathplanner/paths/I_Load.path index 2b80b9c..ead729e 100644 --- a/src/main/deploy/pathplanner/paths/I_Load.path +++ b/src/main/deploy/pathplanner/paths/I_Load.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.70625, - "y": 7.31075 + "x": 1.7607730263157892, + "y": 7.3541118421052625 }, "prevControl": { - "x": 2.9828827328027745, - "y": 6.907224661476541 + "x": 3.0374057591185633, + "y": 6.950586503581804 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/K_Load.path b/src/main/deploy/pathplanner/paths/K_Load.path index ac6599b..02d7a92 100644 --- a/src/main/deploy/pathplanner/paths/K_Load.path +++ b/src/main/deploy/pathplanner/paths/K_Load.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6965000000000001, - "y": 7.31075 + "x": 1.7607730263157892, + "y": 7.3541118421052625 }, "prevControl": { - "x": 2.526149628904741, - "y": 6.55814403518892 + "x": 2.5904226552205305, + "y": 6.601505877294183 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/L_Load.path b/src/main/deploy/pathplanner/paths/L_Load.path index ac316b8..cb1a233 100644 --- a/src/main/deploy/pathplanner/paths/L_Load.path +++ b/src/main/deploy/pathplanner/paths/L_Load.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.9762538718454719, + "x": 1.9762538718454716, "y": 5.874670105580411 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.6965000000000001, - "y": 7.31075 + "x": 1.7607730263157892, + "y": 7.3541118421052625 }, "prevControl": { - "x": 2.125692970026915, - "y": 6.708617685551795 + "x": 2.1899659963427043, + "y": 6.751979527657058 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Load_C.path b/src/main/deploy/pathplanner/paths/Load_C.path index d5ee63b..a966481 100644 --- a/src/main/deploy/pathplanner/paths/Load_C.path +++ b/src/main/deploy/pathplanner/paths/Load_C.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.6672500000000001, - "y": 0.6807499999999993 + "x": 1.693421052631579, + "y": 0.7151315789473688 }, "prevControl": null, "nextControl": { - "x": 2.335595428956344, - "y": 1.1592541578848965 + "x": 2.3617664815879227, + "y": 1.1936357368322659 }, "isLocked": false, "linkedName": "RightLoad" diff --git a/src/main/deploy/pathplanner/paths/Load_D.path b/src/main/deploy/pathplanner/paths/Load_D.path index d4e7227..529a009 100644 --- a/src/main/deploy/pathplanner/paths/Load_D.path +++ b/src/main/deploy/pathplanner/paths/Load_D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.6672500000000001, - "y": 0.6807499999999993 + "x": 1.693421052631579, + "y": 0.7151315789473688 }, "prevControl": null, "nextControl": { - "x": 2.164634098905838, - "y": 0.8286216643092434 + "x": 2.190805151537417, + "y": 0.8630032432566129 }, "isLocked": false, "linkedName": "RightLoad" diff --git a/src/main/deploy/pathplanner/paths/Load_K.path b/src/main/deploy/pathplanner/paths/Load_K.path index 6d5bd23..e711ad5 100644 --- a/src/main/deploy/pathplanner/paths/Load_K.path +++ b/src/main/deploy/pathplanner/paths/Load_K.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.6965000000000001, - "y": 7.31075 + "x": 1.7607730263157892, + "y": 7.3541118421052625 }, "prevControl": null, "nextControl": { - "x": 2.1863951269130357, - "y": 7.179789992891861 + "x": 2.2506681532288253, + "y": 7.2231518349971235 }, "isLocked": false, "linkedName": "LeftLoad" diff --git a/src/main/deploy/pathplanner/paths/Load_L.path b/src/main/deploy/pathplanner/paths/Load_L.path index 5d2f832..a25e956 100644 --- a/src/main/deploy/pathplanner/paths/Load_L.path +++ b/src/main/deploy/pathplanner/paths/Load_L.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.6965000000000001, - "y": 7.31075 + "x": 1.7607730263157892, + "y": 7.3541118421052625 }, "prevControl": null, "nextControl": { - "x": 2.422633701194429, - "y": 6.55509009009426 + "x": 2.4869067275102186, + "y": 6.598451932199523 }, "isLocked": false, "linkedName": "LeftLoad" diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index a5f2c87..208305b 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -120,271 +120,10 @@ public static class AlignmentConfig { public static final LinearAcceleration MAX_ALIGN_TRANSLATION_ACCELERATION = MetersPerSecondPerSecond.of(6.0); public static final AngularVelocity MAX_ALIGN_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75); public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(6.0 * Math.PI); - - // //April Tag IDs - // public static final double id1 = 1; - // public static final double id2 = 2; - // public static final double id3 = 3; - // public static final double id4 = 4; - // public static final double id5 = 5; - // public static final double id6 = 6; - // public static final double id7 = 7; - // public static final double id8 = 8; - // public static final double id9 = 9; - // public static final double id10 = 10; - // public static final double id11 = 11; - // public static final double id12 = 12; - // public static final double id13 = 13; - // public static final double id14 = 14; - // public static final double id15 = 15; - // public static final double id16 = 16; - // public static final double id17 = 17; - // public static final double id18 = 18; - // public static final double id19 = 19; - // public static final double id20 = 20; - // public static final double id21 = 21; - // public static final double id22 = 22; - - // //Joey's Pose Constants for the Reef Locations - // public static final Pose2d Error = new Pose2d(6, 6, Rotation2d.fromDegrees(0)); - - // public static final Pose2d Ablue = new Pose2d(3.180, 4.175, Rotation2d.fromDegrees(0)); - // public static final Pose2d Bblue = new Pose2d(3.180, 3.850, Rotation2d.fromDegrees(0)); - // public static final Pose2d Cblue = new Pose2d(3.685, 2.975, Rotation2d.fromDegrees(60)); - // public static final Pose2d Dblue = new Pose2d(3.975, 2.825, Rotation2d.fromDegrees(60)); - // public static final Pose2d Eblue = new Pose2d(5.000, 2.825, Rotation2d.fromDegrees(120)); - // public static final Pose2d Fblue = new Pose2d(5.285, 2.975, Rotation2d.fromDegrees(120)); - // public static final Pose2d Gblue = new Pose2d(5.8, 3.850, Rotation2d.fromDegrees(180)); - // public static final Pose2d Hblue = new Pose2d(5.8, 4.175, Rotation2d.fromDegrees(180)); - // public static final Pose2d Iblue = new Pose2d(5.285, 5.075, Rotation2d.fromDegrees(240)); - // public static final Pose2d Jblue = new Pose2d(5.000, 5.230, Rotation2d.fromDegrees(240)); - // public static final Pose2d Kblue = new Pose2d(3.975, 5.230, Rotation2d.fromDegrees(300)); - // public static final Pose2d Lblue = new Pose2d(3.685, 5.075, Rotation2d.fromDegrees(300)); - - // public static final double fieldFlip = 17.5; - // public static final double fieldFlipy = 8; - - // public static final Pose2d Ared = new Pose2d(fieldFlip - 3.180, fieldFlipy - 4.175, Rotation2d.fromDegrees(180)); - // public static final Pose2d Bred = new Pose2d(fieldFlip - 3.180, fieldFlipy - 3.850, Rotation2d.fromDegrees(180)); - // public static final Pose2d Cred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 2.975, Rotation2d.fromDegrees(-120)); - // public static final Pose2d Dred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 2.825, Rotation2d.fromDegrees(-120)); - // public static final Pose2d Ered = new Pose2d(fieldFlip - 5.000, fieldFlipy - 2.825, Rotation2d.fromDegrees(-60)); - // public static final Pose2d Fred = new Pose2d(fieldFlip - 5.285, fieldFlipy - 2.975, Rotation2d.fromDegrees(-60)); - // public static final Pose2d Gred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 3.850, Rotation2d.fromDegrees(0)); - // public static final Pose2d Hred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 4.175, Rotation2d.fromDegrees(0)); - // public static final Pose2d Ired = new Pose2d(fieldFlip - 5.285, fieldFlipy - 5.075, Rotation2d.fromDegrees(-300)); - // public static final Pose2d Jred = new Pose2d(fieldFlip - 5.000, fieldFlipy - 5.230, Rotation2d.fromDegrees(-300)); - // public static final Pose2d Kred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 5.230, Rotation2d.fromDegrees(-240)); - // public static final Pose2d Lred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 5.075, Rotation2d.fromDegrees(-240)); - - - //TODO: MIGHT NEED TO PLAY AROUND WITH ALL THE POSES BELOW - - // /** Pose of the robot relative to a reef branch for scoring coral on L4 */ - // public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L4 = new Transform2d( - // Units.inchesToMeters(-40), - // Units.inchesToMeters(12), - // Rotation2d.fromDegrees(-90)); - // /** Pose of the robot relative to a reef branch for scoring coral on L3 */ - // public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L3 = new Transform2d( - // Units.inchesToMeters(-40), - // Units.inchesToMeters(12), - // Rotation2d.fromDegrees(-90)); - - // /** Pose of the robot relative to a reef branch for scoring coral on L2 */ - // public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L2 = new Transform2d( - // Units.inchesToMeters(-40), - // Units.inchesToMeters(12), - // Rotation2d.fromDegrees(90)); - - // /** Pose of the robot relative to the reef trough for scoring coral on L1 */ - // public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L1 = new Transform2d( - // Units.inchesToMeters(-20), - // Units.inchesToMeters(-8), - // Rotation2d.fromDegrees(90)); - - // // spotless:off - // /* The reef branches are in the arrays like this: - // * ---------------------------------------- - // * | 5 / \ 6 | 11 / \ 0 | - // * B 4 / \ 7 | 10 / \ 1 | - // * L 3 | | 8 | 9 | | 2 R - // * +X U 2 | | 9 | 8 | | 3 E - // * E 1 \ / 10 | 7 \ / 4 D - // * | 0 \ / 11 | 6 \ / 5 | - // * |___________________|__________________| - // * (0, 0) +Y - // */ - // // spotless:on - // /** - // * Poses of the right branches on the blue reef. Translation is the branch pipe base, rotation is pointing toward - // * reef center. - // */ - // public static final List REEF_BRANCH_POSES_BLUE_RIGHT = Stream - // .of( - // new Pose2d(4.347746, 3.467, Rotation2d.fromDegrees(60)), // 0 - // new Pose2d(3.942648, 3.840490, Rotation2d.fromDegrees(0)), // 2 - // new Pose2d(4.062584, 4.398912, Rotation2d.fromDegrees(-60)), // 4 - // new Pose2d(4.588763, 4.542161, Rotation2d.fromDegrees(-120)), // 6 - // new Pose2d(4.98, 4.215, Rotation2d.fromDegrees(180)), // 8 - // new Pose2d(4.873353, 3.632614, Rotation2d.fromDegrees(120))) // 10 - // .collect(Collectors.toUnmodifiableList()); - - // /** - // * Poses of the left branches on the blue reef. Translation is the branch pipe base, rotation is pointing toward - // * reef center. - // */ - // public static final List REEF_BRANCH_POSES_BLUE_LEFT = Stream - // .of( - // new Pose2d(4.062584, 3.630770, Rotation2d.fromDegrees(60)), // 1 - // new Pose2d(3.942648, 4.169106, Rotation2d.fromDegrees(0)), // 3 - // new Pose2d(4.347175, 4.515, Rotation2d.fromDegrees(-60)), // 5 - // new Pose2d(4.873926, 4.378820, Rotation2d.fromDegrees(-120)), // 7 - // new Pose2d(4.994328, 3.841097, Rotation2d.fromDegrees(180)), // 9 - // new Pose2d(4.589334, 3.466500, Rotation2d.fromDegrees(120)))// 11 - // .collect(Collectors.toUnmodifiableList()); - - // /** - // * Poses of the right branches on the red reef. Translation is the branch pipe base, rotation is pointing toward - // * reef center. - // */ - // public static final List REEF_BRANCH_POSES_RED_RIGHT = Stream - // .of( - // new Pose2d(13.200254, 4.585000, Rotation2d.fromDegrees(-120)), // 0 - // new Pose2d(13.605352, 4.211510, Rotation2d.fromDegrees(-180)), // 2 - // new Pose2d(13.485416, 3.653088, Rotation2d.fromDegrees(120)), // 4 - // new Pose2d(12.959237, 3.509839, Rotation2d.fromDegrees(60)), // 6 - // new Pose2d(12.568000, 3.837000, Rotation2d.fromDegrees(0)), // 8 - // new Pose2d(12.598000, 4.292000, Rotation2d.fromDegrees(-60))) // 10 - // .collect(Collectors.toUnmodifiableList()); - - // /** - // * Poses of the left branches on the red reef. Translation is the branch pipe base, rotation is pointing toward reef - // * center. - // */ - // public static final List REEF_BRANCH_POSES_RED_LEFT = Stream - // .of( - // new Pose2d(13.485416, 4.421230, Rotation2d.fromDegrees(-120)), // 1 - // new Pose2d(13.605352, 3.882894, Rotation2d.fromDegrees(-180)), // 3 - // new Pose2d(13.200825, 3.537000, Rotation2d.fromDegrees(120)), // 5 - // new Pose2d(12.674074, 3.673180, Rotation2d.fromDegrees(60)), // 7 - // new Pose2d(12.553672, 4.210903, Rotation2d.fromDegrees(0)), // 9 - // new Pose2d(12.958666, 4.585500, Rotation2d.fromDegrees(-60)))// 11 - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L4 left branches on the red alliance */ - // public static final List REEF_L4_SCORE_POSES_RED_LEFT = REEF_BRANCH_POSES_RED_LEFT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L4)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L4 right branches on the red alliance */ - // public static final List REEF_L4_SCORE_POSES_RED_RIGHT = REEF_BRANCH_POSES_RED_RIGHT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L4)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L4 left branches on the blue alliance */ - // public static final List REEF_L4_SCORE_POSES_BLUE_LEFT = REEF_BRANCH_POSES_BLUE_LEFT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L4)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L4 right branches on the blue alliance */ - // public static final List REEF_L4_SCORE_POSES_BLUE_RIGHT = REEF_BRANCH_POSES_BLUE_RIGHT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L4)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L3 left branch on the red alliance */ - // public static final List REEF_L3_SCORE_POSES_RED_LEFT = REEF_BRANCH_POSES_RED_LEFT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L3)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L3 right branch on the red alliance */ - // public static final List REEF_L3_SCORE_POSES_RED_RIGHT = REEF_BRANCH_POSES_RED_RIGHT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L3)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L3 left branch on the blue alliance */ - // public static final List REEF_L3_SCORE_POSES_BLUE_LEFT = REEF_BRANCH_POSES_BLUE_LEFT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L3)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L3 right branch on the blue alliance */ - // public static final List REEF_L3_SCORE_POSES_BLUE_RIGHT = REEF_BRANCH_POSES_BLUE_RIGHT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L3)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L2 left branch on the red alliance */ - // public static final List REEF_L2_SCORE_POSES_RED_LEFT = REEF_BRANCH_POSES_RED_LEFT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L2)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L2 right branch on the red alliance */ - // public static final List REEF_L2_SCORE_POSES_RED_RIGHT = REEF_BRANCH_POSES_RED_RIGHT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L2)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L2 left branch on the blue alliance */ - // public static final List REEF_L2_SCORE_POSES_BLUE_LEFT = REEF_BRANCH_POSES_BLUE_LEFT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L2)) - // .collect(Collectors.toUnmodifiableList()); - - // /** Poses of the robot for scoring on L2 right on the blue alliance */ - // public static final List REEF_L2_SCORE_POSES_BLUE_RIGHT = REEF_BRANCH_POSES_BLUE_RIGHT.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L2)) - // .collect(Collectors.toUnmodifiableList()); - - // // spotless:off - // /* The reef level 1 positions are in the arrays like this: - // * ---------------------------------------- - // * | / \ 3 | / \ 0 | - // * B 2 / \ | 5 / \ | - // * L | | 4 | | | 1 R - // * +X U 1 | | | 4 | | E - // * E \ / 5 | \ / 2 D - // * | 0 \ / | 3 \ / | - // * |___________________|__________________| - // * (0, 0) +Y - // */ - // // spotless:on - // public static final List REEF_L1_POSES_BLUE = Stream - // .of( - // new Pose2d(3.66, 3.540630, Rotation2d.fromDegrees(-30)), - // new Pose2d(3.66, 4.496321, Rotation2d.fromDegrees(-90)), - // new Pose2d(4.480743, 4.981, Rotation2d.fromDegrees(-150)), - // new Pose2d(5.312527, 4.511019, Rotation2d.fromDegrees(150)), - // new Pose2d(5.321074, 3.555505, Rotation2d.fromDegrees(90)), - // new Pose2d(4.497811, 3.070409, Rotation2d.fromDegrees(30))) - // .collect(Collectors.toUnmodifiableList()); - - // public static final List REEF_L1_POSES_RED = Stream - // .of( - // new Pose2d(13.89, 4.51, Rotation2d.fromDegrees(150)), - // new Pose2d(13.89, 3.56, Rotation2d.fromDegrees(90)), - // new Pose2d(13.07, 3.07, Rotation2d.fromDegrees(30)), - // new Pose2d(12.24, 3.54, Rotation2d.fromDegrees(-30)), - // new Pose2d(12.23, 4.50, Rotation2d.fromDegrees(-90)), - // new Pose2d(13.05, 4.98, Rotation2d.fromDegrees(-150))) - // .collect(Collectors.toUnmodifiableList()); - - // public static final List REEF_L1_SCORE_POSES_BLUE = REEF_L1_POSES_BLUE.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L1)) - // .collect(Collectors.toUnmodifiableList()); - - // public static final List REEF_L1_SCORE_POSES_RED = REEF_L1_POSES_RED.stream() - // .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L1)) - // .collect(Collectors.toUnmodifiableList()); - - // public static final Distance DISTANCE_TARGET_L4 = Meters.of(0.34); - // public static final Distance DISTANCE_TARGET_L3 = Meters.of(0.34); - - // public static final Distance LATERAL_TARGET_L3_LEFT = Meters.of(0.05); - // public static final Distance LATERAL_TARGET_L3_RIGHT = Meters.of(0.02); - - // public static final Distance LATERAL_TARGET_L4_LEFT = Meters.of(0.05); - // public static final Distance LATERAL_TARGET_L4_RIGHT = Meters.of(0.02); } - private static final double acutalX = 6.25; + private static final double acutalX = 6.175; private static final double x = 0.5 * acutalX; private static final double y = (Math.sqrt(3)/2) * acutalX; diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java index 62a7812..9833af2 100644 --- a/src/main/java/frc/robot/vision/commands/AutoAlign.java +++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java @@ -42,11 +42,11 @@ public class AutoAlign extends Command { private final Angle THETA_TOLERANCE = Degrees.of(1.0); private final double branchOffsetRightWrist = Units.inchesToMeters(6.469); - private final Transform2d leftBranchTransformRightWrist = new Transform2d(0.0, -branchOffsetRightWrist, Rotation2d.kZero); - private final Transform2d rightBranchTransformRightWrist = new Transform2d(0.0, branchOffsetRightWrist, Rotation2d.kZero); + private final Transform2d leftBranchTransformRightWrist = new Transform2d(0.0, -branchOffsetRightWrist - Units.inchesToMeters(0.75), Rotation2d.kZero); + private final Transform2d rightBranchTransformRightWrist = new Transform2d(0.0, branchOffsetRightWrist - Units.inchesToMeters(2.0), Rotation2d.kZero); private final double branchOffsetLeftWrist = Units.inchesToMeters(6.469); - private final Transform2d leftBranchTransformLeftWrist = new Transform2d(0.0, -branchOffsetLeftWrist , Rotation2d.kZero); + private final Transform2d leftBranchTransformLeftWrist = new Transform2d(0.0, -branchOffsetLeftWrist + Units.inchesToMeters(1.25) , Rotation2d.kZero); private final Transform2d rightBranchTransformLeftWrist = new Transform2d(0.0, branchOffsetLeftWrist, Rotation2d.kZero); public final Transform2d robotOffset = new Transform2d(0.3018, 0, Rotation2d.kZero);