diff --git a/docs/README.md b/docs/README.md index 9e4fd88f..b7c2cd28 100644 --- a/docs/README.md +++ b/docs/README.md @@ -13,3 +13,9 @@ [Autos](/docs/autos/CalculatedFieldPos.md) [WPILib rotations](/docs/rotations/rotations.md) + +[CANID](/docs/CANID/CANID.md) + +[Controller Mappings](/docs/ControllerMapping/controller-mapping.md) + +[Reef Standard Names/Id Given](/docs/reefFaceStandard/reef.md) diff --git a/docs/reefFaceStandard/reef.md b/docs/reefFaceStandard/reef.md new file mode 100644 index 00000000..bc879490 --- /dev/null +++ b/docs/reefFaceStandard/reef.md @@ -0,0 +1 @@ +![reef](/docs/reefFaceStandard/reef.png) diff --git a/docs/reefFaceStandard/reef.png b/docs/reefFaceStandard/reef.png new file mode 100644 index 00000000..28f4d73c Binary files /dev/null and b/docs/reefFaceStandard/reef.png differ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2aa9e6d7..e4717813 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -426,6 +426,9 @@ public void periodic() { .getPose("Main") .getTranslation() .getDistance(getTarget().getTranslation())); + Logger.recordOutput("Pole", getPoleID()); + Logger.recordOutput("Coral Offset", getCoralOffset(getPoleID())); + Logger.recordOutput("target", getTarget()); } }); @@ -444,6 +447,39 @@ public void periodic() { driveController); } + public int getPoleID() { + Pose2d pose = + DistanceManager.getNearestPosition( + RobotOdometry.instance.getPose("Main"), + AllianceManager.chooseFromAlliance( + FieldConstants.reefPositionsBlue, FieldConstants.reefPositionsRed)); + int poleID = 0; + for (int i = 0; + i + < AllianceManager.chooseFromAlliance( + FieldConstants.reefPositionsBlue.length, FieldConstants.reefPositionsRed.length); + i++) { + if (pose.getTranslation() + .getDistance( + AllianceManager.chooseFromAlliance( + FieldConstants.reefPositionsBlue[i], FieldConstants.reefPositionsRed[i]) + .getTranslation()) + < 0.2) { + poleID = i; + } + } + + poleID = (poleID * 2) + (coralPreset.isRight() ? 0 : 1); + + return poleID; + } + + public double getCoralOffset(int poleID) { + return AllianceManager.chooseFromAlliance( + FieldConstants.reefPoleRegularHeight - FieldConstants.reefPoleHeightBlue[poleID], + FieldConstants.reefPoleRegularHeight - FieldConstants.reefPoleHeightRed[poleID]); + } + public Pose2d coralAdjust(Pose2d pose, Supplier preset) { if (algaeMode) { return pose; @@ -915,9 +951,10 @@ public Command setupAutoPlace(Supplier coralPreset) { (new InstantCommand( () -> { presetActive = - algaeMode - ? coralPreset.get().getLiftAlgae() - : coralPreset.get().getLift(); + (algaeMode + ? coralPreset.get().getLiftAlgae() + : coralPreset.get().getLift()) + + (coralPreset.get().isL4() ? getCoralOffset(getPoleID()) : 0); gantryPresetActive = coralPreset.get(); }) .andThen( diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 645d0fd5..27f087d8 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -34,24 +34,25 @@ public class FieldConstants { public static final int kSouthWest = 4; public static final int kNorthWest = 5; + public static final double reefPoleRegularHeight = 0.568; // TODO Switch from fake units to meters public static final Pose2d[] reefPositionsRed = new Pose2d[] { - new Pose2d(12.227305999999999, 4.0259, Rotation2d.fromDegrees(360)), - new Pose2d(12.643358, 4.745482, Rotation2d.fromDegrees(300)), - new Pose2d(13.474446, 4.745482, Rotation2d.fromDegrees(240)), - new Pose2d(13.890498, 4.0259, Rotation2d.fromDegrees(180)), - new Pose2d(13.474446, 3.3063179999999996, Rotation2d.fromDegrees(120)), - new Pose2d(12.643358, 3.3063179999999996, Rotation2d.fromDegrees(60)), + new Pose2d(13.890498, 4.0259, Rotation2d.fromDegrees(180)), // id 7 + new Pose2d(13.474446, 3.3063179999999996, Rotation2d.fromDegrees(120)), // id 6 + new Pose2d(12.643358, 3.3063179999999996, Rotation2d.fromDegrees(60)), // id 11 + new Pose2d(12.227305999999999, 4.0259, Rotation2d.fromDegrees(360)), // id 10 + new Pose2d(12.643358, 4.745482, Rotation2d.fromDegrees(300)), // id 9 + new Pose2d(13.474446, 4.745482, Rotation2d.fromDegrees(240)), // id 8 }; public static final Pose2d[] reefPositionsBlue = new Pose2d[] { - new Pose2d(5.321046, 4.0259, Rotation2d.fromDegrees(180)), - new Pose2d(4.904739999999999, 3.3063179999999996, Rotation2d.fromDegrees(480)), - new Pose2d(4.073905999999999, 3.3063179999999996, Rotation2d.fromDegrees(420)), - new Pose2d(3.6576, 4.0259, Rotation2d.fromDegrees(360)), - new Pose2d(4.073905999999999, 4.745482, Rotation2d.fromDegrees(300)), - new Pose2d(4.904739999999999, 4.745482, Rotation2d.fromDegrees(240)), + new Pose2d(3.6576, 4.0259, Rotation2d.fromDegrees(360)), // id 18 + new Pose2d(4.073905999999999, 4.745482, Rotation2d.fromDegrees(300)), // id 19 + new Pose2d(4.904739999999999, 4.745482, Rotation2d.fromDegrees(240)), // id 20 + new Pose2d(5.321046, 4.0259, Rotation2d.fromDegrees(180)), // id 21 + new Pose2d(4.904739999999999, 3.3063179999999996, Rotation2d.fromDegrees(480)), // id 22 + new Pose2d(4.073905999999999, 3.3063179999999996, Rotation2d.fromDegrees(420)), // id 17 }; public static final Translation2d reefCenterPosRed = new Translation2d(13.07, 4); @@ -75,4 +76,11 @@ public class FieldConstants { new Pose2d(16.697198, 0.65532, Rotation2d.fromDegrees(126)), // 1 new Pose2d(16.697198, 7.3964799999999995, Rotation2d.fromDegrees(234)) // 2 }; + // left is +1 id + public static final double[] reefPoleHeightBlue = { + 1.825, 1.821, 1.816, 1.812, 1.806, 1.811, 1.820, 1.820, 1.825, 1.825, 1.822, 1.825, + }; // TODO Convert the lift from FakeUnit to Meters + public static final double[] reefPoleHeightRed = { + 1.825, 1.821, 1.816, 1.812, 1.806, 1.811, 1.820, 1.820, 1.825, 1.825, 1.822, 1.825, + }; /// TODO Convert the lift from FakeUnit to Meters } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 2e934c04..b0e16b11 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -317,6 +317,10 @@ public double getLiftAlgae() { return liftAlgae; } + public boolean isL4() { + return this == LeftL4 || this == RightL4; + } + public double getGantry(boolean dsSide) { switch (gantrySetpoint) { case LEFT: