diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 176d356a..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": 11.998469387755103, - "y": 5.416613520408163 + "x": 5.312462093955592, + "y": 5.435106779399671 }, "prevControl": { - "x": 11.748469387755103, - "y": 6.916613520408162 + "x": 5.411975523921398, + "y": 5.664447306250991 }, "nextControl": null, "isLocked": false, @@ -58,7 +58,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -120.44045737256441 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 619491a0..7238d265 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ 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 = 78; - public static final String GIT_SHA = "89ac08633e9c2430feef390019719ac47b439453"; - public static final String GIT_DATE = "2025-04-02 19:23:09 EDT"; - public static final String GIT_BRANCH = "205-algae-pop-shot"; - public static final String BUILD_DATE = "2025-04-02 20:01:00 EDT"; - public static final long BUILD_UNIX_TIME = 1743638460991L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 82; + public static final String GIT_SHA = "ac86fc677adff1afe3a1c4838b99aa071515d689"; + public static final String GIT_DATE = "2025-04-02 20:09:46 EDT"; + public static final String GIT_BRANCH = "algae-otf"; + public static final String BUILD_DATE = "2025-04-02 20:10:13 EDT"; + public static final long BUILD_UNIX_TIME = 1743639013691L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index c682b27b..7dc2b0e4 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -192,7 +192,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) { @@ -214,11 +214,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) { diff --git a/src/main/java/frc/robot/constants/field/BlueFieldLocations.java b/src/main/java/frc/robot/constants/field/BlueFieldLocations.java index 6f62290c..1258c492 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); 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); 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 8c4767f7..53d2d419 100644 --- a/src/main/java/frc/robot/subsystems/drive/states/OTFState.java +++ b/src/main/java/frc/robot/subsystems/drive/states/OTFState.java @@ -61,7 +61,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, @@ -69,6 +68,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( @@ -78,7 +85,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, @@ -86,6 +92,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( @@ -95,7 +109,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, @@ -103,6 +116,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( @@ -112,7 +133,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, @@ -120,6 +140,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( @@ -129,7 +157,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, @@ -137,6 +164,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( @@ -146,7 +181,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, @@ -154,6 +188,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(