From 65cd13357c7a7a8d01f4725dd8586dfd9e17d1fb Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Wed, 26 Mar 2025 09:28:44 -0400 Subject: [PATCH 1/4] add red algae otf locations --- .../deploy/pathplanner/paths/Example Path.path | 10 +++++----- .../constants/field/RedFieldLocations.java | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 176d356a..79bf3562 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 11.998469387755103, - "y": 5.416613520408163 + "x": 12.20250886615954, + "y": 2.6268451891447366 }, "prevControl": { - "x": 11.748469387755103, - "y": 6.916613520408162 + "x": 11.95250886615954, + "y": 4.126845189144735 }, "nextControl": null, "isLocked": false, @@ -58,7 +58,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 59.554619709648165 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/constants/field/RedFieldLocations.java b/src/main/java/frc/robot/constants/field/RedFieldLocations.java index 12c017c5..606d89e4 100644 --- a/src/main/java/frc/robot/constants/field/RedFieldLocations.java +++ b/src/main/java/frc/robot/constants/field/RedFieldLocations.java @@ -58,6 +58,24 @@ public class RedFieldLocations { public Translation2d redReefOTF11Translation = new Translation2d(12.2, 2.6); public Rotation2d redReefOTF11Rotation = new Rotation2d(Math.toRadians(60)); + public Translation2d redReefAlgaeOTF0Translation = new Translation2d(11.403, 4.049); + public Rotation2d redReefAlgaeOTF0Rotation = new Rotation2d(Math.toRadians(0)); + + public Translation2d redReefAlgaeOTF1Translation = new Translation2d(12.232, 5.421); + public Rotation2d redReefAlgaeOTF1Rotation = new Rotation2d(Math.toRadians(-60)); + + public Translation2d redReefAlgaeOTF2Translation = new Translation2d(13.871, 5.421); + public Rotation2d redReefAlgaeOTF2Rotation = new Rotation2d(Math.toRadians(-120)); + + public Translation2d redReefAlgaeOTF3Translation = new Translation2d(14.673, 4.025); + public Rotation2d redReefAlgaeOTF3Rotation = new Rotation2d(Math.toRadians(180)); + + public Translation2d redReefAlgaeOTF4Translation = new Translation2d(13.921, 2.637); + public Rotation2d redReefAlgaeOTF4Rotation = new Rotation2d(Math.toRadians(120)); + + public Translation2d redReefAlgaeOTF5Translation = new Translation2d(12.203, 2.627); + public Rotation2d redReefAlgaeOTF5Rotation = new Rotation2d(Math.toRadians(120)); + public Translation2d redReef0Translation = new Translation2d(12.273, 3.867); public Translation2d redReef1Translation = new Translation2d(12.273, 4.187); From 6c99c24e14e97606e0f8f1dff6c5e1bad70d0ca5 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Wed, 26 Mar 2025 09:33:51 -0400 Subject: [PATCH 2/4] add blue algae otf locations --- .../pathplanner/paths/Example Path.path | 30 +++++++++---------- .../constants/field/BlueFieldLocations.java | 18 +++++++++++ 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 79bf3562..ae6a8296 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 4.828993187904608, - "y": 4.608670175573618 + "x": 4.853401264391447, + "y": 6.951653731496711 }, "prevControl": null, "nextControl": { - "x": 5.842276098080277, - "y": 4.136168594681116 + "x": 5.093757985115818, + "y": 6.882888574602928 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.166769560390973, - "y": 5.0964860911203305 + "x": 6.606582159745066, + "y": 7.230570582339637 }, "prevControl": { - "x": 4.166769560390973, - "y": 6.0964860911203305 + "x": 6.151317312325627, + "y": 7.799255624469424 }, "nextControl": { - "x": 6.166769560390973, - "y": 4.0964860911203305 + "x": 7.232336636905039, + "y": 6.4489218053173945 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 12.20250886615954, - "y": 2.6268451891447366 + "x": 5.312462093955592, + "y": 5.435106779399671 }, "prevControl": { - "x": 11.95250886615954, - "y": 4.126845189144735 + "x": 5.411975523921398, + "y": 5.664447306250991 }, "nextControl": null, "isLocked": false, @@ -58,7 +58,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.554619709648165 + "rotation": -120.44045737256441 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/constants/field/BlueFieldLocations.java b/src/main/java/frc/robot/constants/field/BlueFieldLocations.java index 6f62290c..a8a06c03 100644 --- a/src/main/java/frc/robot/constants/field/BlueFieldLocations.java +++ b/src/main/java/frc/robot/constants/field/BlueFieldLocations.java @@ -58,6 +58,24 @@ public class BlueFieldLocations { public Translation2d blueReefOTF11Translation = new Translation2d(5.5, 5.6); public Rotation2d blueReefOTF11Rotation = new Rotation2d(Math.toRadians(-120)); + public Translation2d blueReefAlgaeOTF0Translation = new Translation2d(6.102, 4.034); + public Rotation2d blueReefAlgaeOTF0Rotation = new Rotation2d(Math.toRadians(-180)); + + public Translation2d blueReefAlgaeOTF1Translation = new Translation2d(5.322, 2.647); + public Rotation2d blueReefAlgaeOTF1Rotation = new Rotation2d(Math.toRadians(120)); + + public Translation2d blueReefAlgaeOTF2Translation = new Translation2d(3.667, 2.621); + public Rotation2d blueReefAlgaeOTF2Rotation = new Rotation2d(Math.toRadians(60)); + + public Translation2d blueReefAlgaeOTF3Translation = new Translation2d(2.879, 4.001); + public Rotation2d blueReefAlgaeOTF3Rotation = new Rotation2d(Math.toRadians(0)); + + public Translation2d blueReefAlgaeOTF4Translation = new Translation2d(3.661, 5.435); + public Rotation2d blueReefAlgaeOTF4Rotation = new Rotation2d(Math.toRadians(-60)); + + public Translation2d blueReefAlgaeOTF5Translation = new Translation2d(5.312, 5.435); + public Rotation2d blueReefAlgaeOTF5Rotation = new Rotation2d(Math.toRadians(-120)); + public Translation2d blueReef0Translation = new Translation2d(5.285, 4.186); public Translation2d blueReef1Translation = new Translation2d(5.285, 3.861); From 7407684e07979bdb726b2bb37c6ea873b71580e5 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Wed, 26 Mar 2025 09:37:29 -0400 Subject: [PATCH 3/4] add algae otf locations to switch statement --- src/main/java/frc/robot/BuildConstants.java | 12 ++--- .../constants/field/BlueFieldLocations.java | 2 +- .../subsystems/drive/states/OTFState.java | 54 ++++++++++++++++--- 3 files changed, 55 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index b95248f2..2d93770b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 76; - public static final String GIT_SHA = "ec9ec0e8331b948fcb446e736cebc91f5f9976e0"; - public static final String GIT_DATE = "2025-03-21 08:17:41 EDT"; - public static final String GIT_BRANCH = "vabla-climb"; - public static final String BUILD_DATE = "2025-03-21 18:53:50 EDT"; - public static final long BUILD_UNIX_TIME = 1742597630171L; + public static final int GIT_REVISION = 71; + public static final String GIT_SHA = "6c99c24e14e97606e0f8f1dff6c5e1bad70d0ca5"; + public static final String GIT_DATE = "2025-03-26 09:33:51 EDT"; + public static final String GIT_BRANCH = "algae-otf"; + public static final String BUILD_DATE = "2025-03-26 09:37:15 EDT"; + public static final long BUILD_UNIX_TIME = 1742996235953L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/constants/field/BlueFieldLocations.java b/src/main/java/frc/robot/constants/field/BlueFieldLocations.java index a8a06c03..1258c492 100644 --- a/src/main/java/frc/robot/constants/field/BlueFieldLocations.java +++ b/src/main/java/frc/robot/constants/field/BlueFieldLocations.java @@ -72,7 +72,7 @@ public class BlueFieldLocations { public Translation2d blueReefAlgaeOTF4Translation = new Translation2d(3.661, 5.435); public Rotation2d blueReefAlgaeOTF4Rotation = new Rotation2d(Math.toRadians(-60)); - + public Translation2d blueReefAlgaeOTF5Translation = new Translation2d(5.312, 5.435); public Rotation2d blueReefAlgaeOTF5Rotation = new Rotation2d(Math.toRadians(-120)); diff --git a/src/main/java/frc/robot/subsystems/drive/states/OTFState.java b/src/main/java/frc/robot/subsystems/drive/states/OTFState.java index 733cfa44..d8973112 100644 --- a/src/main/java/frc/robot/subsystems/drive/states/OTFState.java +++ b/src/main/java/frc/robot/subsystems/drive/states/OTFState.java @@ -65,7 +65,6 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { JsonConstants.blueFieldLocations.blueReefOTF0Translation, JsonConstants.blueFieldLocations.blueReefOTF0Rotation); case Reef1: - case Algae0: return driveInput.isAllianceRed() ? new Pose2d( JsonConstants.redFieldLocations.redReefOTF1Translation, @@ -73,6 +72,14 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { : new Pose2d( JsonConstants.blueFieldLocations.blueReefOTF1Translation, JsonConstants.blueFieldLocations.blueReefOTF1Rotation); + case Algae0: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReefAlgaeOTF0Translation, + JsonConstants.redFieldLocations.redReefAlgaeOTF0Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReefAlgaeOTF0Translation, + JsonConstants.blueFieldLocations.blueReefAlgaeOTF0Rotation); case Reef2: return driveInput.isAllianceRed() ? new Pose2d( @@ -82,7 +89,6 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { JsonConstants.blueFieldLocations.blueReefOTF2Translation, JsonConstants.blueFieldLocations.blueReefOTF2Rotation); case Reef3: - case Algae1: return driveInput.isAllianceRed() ? new Pose2d( JsonConstants.redFieldLocations.redReefOTF3Translation, @@ -90,6 +96,14 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { : new Pose2d( JsonConstants.blueFieldLocations.blueReefOTF3Translation, JsonConstants.blueFieldLocations.blueReefOTF3Rotation); + case Algae1: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReefAlgaeOTF1Translation, + JsonConstants.redFieldLocations.redReefAlgaeOTF1Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReefAlgaeOTF1Translation, + JsonConstants.blueFieldLocations.blueReefAlgaeOTF1Rotation); case Reef4: return driveInput.isAllianceRed() ? new Pose2d( @@ -99,7 +113,6 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { JsonConstants.blueFieldLocations.blueReefOTF4Translation, JsonConstants.blueFieldLocations.blueReefOTF4Rotation); case Reef5: - case Algae2: return driveInput.isAllianceRed() ? new Pose2d( JsonConstants.redFieldLocations.redReefOTF5Translation, @@ -107,6 +120,14 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { : new Pose2d( JsonConstants.blueFieldLocations.blueReefOTF5Translation, JsonConstants.blueFieldLocations.blueReefOTF5Rotation); + case Algae2: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReefAlgaeOTF2Translation, + JsonConstants.redFieldLocations.redReefAlgaeOTF2Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReefAlgaeOTF2Translation, + JsonConstants.blueFieldLocations.blueReefAlgaeOTF2Rotation); case Reef6: return driveInput.isAllianceRed() ? new Pose2d( @@ -116,7 +137,6 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { JsonConstants.blueFieldLocations.blueReefOTF6Translation, JsonConstants.blueFieldLocations.blueReefOTF6Rotation); case Reef7: - case Algae3: return driveInput.isAllianceRed() ? new Pose2d( JsonConstants.redFieldLocations.redReefOTF7Translation, @@ -124,6 +144,14 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { : new Pose2d( JsonConstants.blueFieldLocations.blueReefOTF7Translation, JsonConstants.blueFieldLocations.blueReefOTF7Rotation); + case Algae3: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReefAlgaeOTF3Translation, + JsonConstants.redFieldLocations.redReefAlgaeOTF3Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReefAlgaeOTF3Translation, + JsonConstants.blueFieldLocations.blueReefAlgaeOTF3Rotation); case Reef8: return driveInput.isAllianceRed() ? new Pose2d( @@ -133,7 +161,6 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { JsonConstants.blueFieldLocations.blueReefOTF8Translation, JsonConstants.blueFieldLocations.blueReefOTF8Rotation); case Reef9: - case Algae4: return driveInput.isAllianceRed() ? new Pose2d( JsonConstants.redFieldLocations.redReefOTF9Translation, @@ -141,6 +168,14 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { : new Pose2d( JsonConstants.blueFieldLocations.blueReefOTF9Translation, JsonConstants.blueFieldLocations.blueReefOTF9Rotation); + case Algae4: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReefAlgaeOTF4Translation, + JsonConstants.redFieldLocations.redReefAlgaeOTF4Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReefAlgaeOTF4Translation, + JsonConstants.blueFieldLocations.blueReefAlgaeOTF4Rotation); case Reef10: return driveInput.isAllianceRed() ? new Pose2d( @@ -150,7 +185,6 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { JsonConstants.blueFieldLocations.blueReefOTF10Translation, JsonConstants.blueFieldLocations.blueReefOTF10Rotation); case Reef11: - case Algae5: return driveInput.isAllianceRed() ? new Pose2d( JsonConstants.redFieldLocations.redReefOTF11Translation, @@ -158,6 +192,14 @@ public static Pose2d findOTFPoseFromDesiredLocation(Drive driveInput) { : new Pose2d( JsonConstants.blueFieldLocations.blueReefOTF11Translation, JsonConstants.blueFieldLocations.blueReefOTF11Rotation); + case Algae5: + return driveInput.isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReefAlgaeOTF5Translation, + JsonConstants.redFieldLocations.redReefAlgaeOTF5Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReefAlgaeOTF5Translation, + JsonConstants.blueFieldLocations.blueReefAlgaeOTF5Rotation); case CoralStationRight: return driveInput.isAllianceRed() ? new Pose2d( From acb6ff97763b9d24a64e08d62676ad771280fdbc Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 26 Mar 2025 18:51:24 -0400 Subject: [PATCH 4/4] make right trigger start otf for coral and algae now --- src/main/java/frc/robot/InitBindings.java | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 40952ca5..7b28e5df 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -198,7 +198,7 @@ public static void initDriveBindings(Drive drive, StrategyManager strategyManage || ScoringSubsystem.getInstance().getGamePiece() == GamePiece.Algae) { DesiredLocation desiredLocation = ReefLineupUtil.getClosestAlgaeLocation(drive.getPose()); - // drive.setDesiredIntakeLocation(desiredLocation); + drive.setDesiredIntakeLocation(desiredLocation); // Set algae level automatically if (ScoringSubsystem.getInstance() != null) { @@ -220,11 +220,13 @@ public static void initDriveBindings(Drive drive, StrategyManager strategyManage } case Mixed: // Start auto align if in mixed autonomy - if (ScoringSubsystem.getInstance() != null - && ScoringSubsystem.getInstance().getGamePiece() == GamePiece.Coral) { - drive.setGoToIntake(true); - drive.fireTrigger(DriveTrigger.BeginOTF); - } + drive.setGoToIntake(true); + drive.fireTrigger(DriveTrigger.BeginOTF); + // if (ScoringSubsystem.getInstance() != null + // && ScoringSubsystem.getInstance().getGamePiece() == GamePiece.Coral) { + // drive.setGoToIntake(true); + // drive.fireTrigger(DriveTrigger.BeginOTF); + // } // Then always start intake for scoring (no break here is intentional) case Manual: if (ScoringSubsystem.getInstance() != null) {