diff --git a/src/main/deploy/auto/1PieceBarge.json b/src/main/deploy/auto/1PieceBarge.json index b96c9477..b9b19c6c 100644 --- a/src/main/deploy/auto/1PieceBarge.json +++ b/src/main/deploy/auto/1PieceBarge.json @@ -6,5 +6,12 @@ "L4" ], "intakeLocation": "CoralStationRight", - "intakeAfterLastScore": false + "intakeAfterLastScore": false, + "startSeededPositionTranslation": { + "x": 11.7, + "y": 4 + }, + "startSeededPositionRotation": { + "degrees": 0 + } } diff --git a/src/main/deploy/auto/1PieceLeft.json b/src/main/deploy/auto/1PieceLeft.json index 9e50579e..2637bb09 100644 --- a/src/main/deploy/auto/1PieceLeft.json +++ b/src/main/deploy/auto/1PieceLeft.json @@ -6,5 +6,12 @@ "L4" ], "intakeLocation": "CoralStationLeft", - "intakeAfterLastScore": false + "intakeAfterLastScore": false, + "startSeededPositionTranslation": { + "x": 11.7, + "y": 1.5 + }, + "startSeededPositionRotation": { + "degrees": -40 + } } diff --git a/src/main/deploy/auto/2PieceLeft.json b/src/main/deploy/auto/2PieceLeft.json index 323eb3ae..e783a033 100644 --- a/src/main/deploy/auto/2PieceLeft.json +++ b/src/main/deploy/auto/2PieceLeft.json @@ -7,5 +7,12 @@ "L4", "L4" ], - "intakeLocation": "CoralStationLeft" + "intakeLocation": "CoralStationLeft", + "startSeededPositionTranslation": { + "x": 11.7, + "y": 1.5 + }, + "startSeededPositionRotation": { + "degrees": -40 + } } diff --git a/src/main/deploy/auto/2PieceRight.json b/src/main/deploy/auto/2PieceRight.json index 9bc57546..a384c008 100644 --- a/src/main/deploy/auto/2PieceRight.json +++ b/src/main/deploy/auto/2PieceRight.json @@ -7,5 +7,12 @@ "L4", "L4" ], - "intakeLocation": "CoralStationRight" + "intakeLocation": "CoralStationRight", + "startSeededPositionTranslation": { + "x": 11.7, + "y": 6.5 + }, + "startSeededPositionRotation": { + "degrees": 40 + } } diff --git a/src/main/deploy/auto/3PieceLeft.json b/src/main/deploy/auto/3PieceLeft.json index d5bfeb45..3b800061 100644 --- a/src/main/deploy/auto/3PieceLeft.json +++ b/src/main/deploy/auto/3PieceLeft.json @@ -9,5 +9,12 @@ "L4", "L4" ], - "intakeLocation": "CoralStationLeft" + "intakeLocation": "CoralStationLeft", + "startSeededPositionTranslation": { + "x": 11.7, + "y": 1.5 + }, + "startSeededPositionRotation": { + "degrees": -40 + } } diff --git a/src/main/deploy/auto/3PieceRight.json b/src/main/deploy/auto/3PieceRight.json index 40a6df77..b8e4d522 100644 --- a/src/main/deploy/auto/3PieceRight.json +++ b/src/main/deploy/auto/3PieceRight.json @@ -9,5 +9,12 @@ "L4", "L4" ], - "intakeLocation": "CoralStationRight" + "intakeLocation": "CoralStationRight", + "startSeededPositionTranslation": { + "x": 11.7, + "y": 6.5 + }, + "startSeededPositionRotation": { + "degrees": 40 + } } diff --git a/src/main/deploy/auto/4PieceLeft.json b/src/main/deploy/auto/4PieceLeft.json index 457cb89a..6b22d841 100644 --- a/src/main/deploy/auto/4PieceLeft.json +++ b/src/main/deploy/auto/4PieceLeft.json @@ -11,5 +11,12 @@ "L4", "L4" ], - "intakeLocation": "CoralStationLeft" + "intakeLocation": "CoralStationLeft", + "startSeededPositionTranslation": { + "x": 11.7, + "y": 1.5 + }, + "startSeededPositionRotation": { + "degrees": -40 + } } diff --git a/src/main/deploy/auto/ExampleAutoPath.json b/src/main/deploy/auto/ExampleAutoPath.json index 2db912bc..0853edb1 100644 --- a/src/main/deploy/auto/ExampleAutoPath.json +++ b/src/main/deploy/auto/ExampleAutoPath.json @@ -7,5 +7,10 @@ "L1", "L1" ], - "intakeLocation": "CoralStationRight" + "intakeLocation": "CoralStationRight", + "startSeededPosition": { + "x": 11.7, + "y": 6.5, + "degrees": 40 + } } diff --git a/src/main/deploy/constants/comp/FeatureFlags.json b/src/main/deploy/constants/comp/FeatureFlags.json index ff6bb68b..52c1a601 100644 --- a/src/main/deploy/constants/comp/FeatureFlags.json +++ b/src/main/deploy/constants/comp/FeatureFlags.json @@ -1,6 +1,6 @@ { "runDrive": true, - "runVision": true, + "runVision": false, "runScoring": true, "runClimb": true, "runRamp": true diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path new file mode 100644 index 00000000..c6db5ad6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.341931818174691, + "y": 6.502940340914731 + }, + "prevControl": null, + "nextControl": { + "x": 5.574119318174691, + "y": 6.173877840914731 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 10.619744318174691, + "y": 6.502940340914731 + }, + "prevControl": { + "x": 11.317753813036703, + "y": 5.944528914045714 + }, + "nextControl": { + "x": 9.92173482331268, + "y": 7.061351767783748 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.198096590901963, + "y": 2.2550426136420048 + }, + "prevControl": { + "x": 10.639687499992872, + "y": 2.544218750005635 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.46003552397868663, + "rotationDegrees": -140.0 + }, + { + "waypointRelativePos": 0.5648312611012444, + "rotationDegrees": -40.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.67317404787988 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -154.85521436932117 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path index 38ad5d4a..68fe5262 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 11.66978181537829, - "y": 6.890164987664473 + "x": 11.67, + "y": 6.89 }, "prevControl": null, "nextControl": { - "x": 12.66978181537829, - "y": 6.890164987664473 + "x": 12.484432232602588, + "y": 6.482997988367408 }, "isLocked": false, "linkedName": null @@ -25,7 +25,7 @@ }, "nextControl": { "x": 17.47167326274671, - "y": 2.930455258018086 + "y": 2.9304552580180863 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 50714f8d..ac55f69b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,14 @@ 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 = 79; public static final String GIT_SHA = "67be46969451c2dd5f8c8669e318a4947be378a5"; public static final String GIT_DATE = "2025-04-03 20:22:38 EDT"; public static final String GIT_BRANCH = "comp/CHCMP"; public static final String BUILD_DATE = "2025-04-06 10:07:35 EDT"; public static final long BUILD_UNIX_TIME = 1743948455918L; + public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/StrategyManager.java b/src/main/java/frc/robot/StrategyManager.java index cfa7fd76..20de1b23 100644 --- a/src/main/java/frc/robot/StrategyManager.java +++ b/src/main/java/frc/robot/StrategyManager.java @@ -1,5 +1,7 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.DoubleSubscriber; @@ -8,6 +10,7 @@ import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.strategies.AutoIntake; @@ -15,6 +18,8 @@ import frc.robot.constants.AutoStrategy; import frc.robot.constants.AutoStrategyContainer.Action; import frc.robot.constants.AutoStrategyContainer.ActionType; +import frc.robot.constants.FeatureFlags; +import frc.robot.constants.VisionConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.Drive.DesiredLocation; import frc.robot.subsystems.scoring.ScoringSubsystem; @@ -435,6 +440,22 @@ public void autonomousInit(AutoStrategy strategy) { this.currentCommand = null; this.currentAction = null; + if (!FeatureFlags.synced.getObject().runVision + || drive.getVisionMeasurementCount() + < VisionConstants.synced.getObject().minimumVisionMeasurementInitCount) { + Pose2d startPose = + new Pose2d(strategy.startSeededPositionTranslation, strategy.startSeededPositionRotation); + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Blue) { + startPose = + new Pose2d( + 17.3736 - startPose.getX(), + 7.9248 - startPose.getY(), + new Rotation2d(startPose.getRotation().getRadians() - Math.PI / 2)); + } + drive.setPose(startPose); + } + actions.clear(); this.addActionsFromAutoStrategy(strategy); System.out.println("New actions loaded: " + String.valueOf(actions.size())); diff --git a/src/main/java/frc/robot/constants/AutoStrategy.java b/src/main/java/frc/robot/constants/AutoStrategy.java index 53cb829b..52ca4fd4 100644 --- a/src/main/java/frc/robot/constants/AutoStrategy.java +++ b/src/main/java/frc/robot/constants/AutoStrategy.java @@ -1,6 +1,8 @@ package frc.robot.constants; import coppercore.parameter_tools.json.JSONExclude; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import frc.robot.subsystems.drive.Drive.DesiredLocation; import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import java.util.List; @@ -11,4 +13,6 @@ public class AutoStrategy { public List scoringLevels = null; public final DesiredLocation intakeLocation = null; public final Boolean intakeAfterLastScore = true; + public final Translation2d startSeededPositionTranslation = null; + public final Rotation2d startSeededPositionRotation = null; } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 828dae52..819b3a01 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -24,4 +24,6 @@ public class VisionConstants { Units.inchesToMeters(-5.5), Units.inchesToMeters(12.0), new Rotation3d(0, 0, 0)); + + public final Integer minimumVisionMeasurementInitCount = 10; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7da32e4d..262217be 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -123,6 +123,8 @@ public class Drive implements DriveTemplate { private ChassisSpeeds goalSpeeds = new ChassisSpeeds(); + private int visionValueCount = 0; + public ProfiledPIDController angleController = new ProfiledPIDController( JsonConstants.drivetrainConstants.rotationAlignKp, @@ -390,6 +392,11 @@ public void autonomousInit() { } } + @AutoLogOutput(key = "Vision/MeasurementCount") + public int getVisionMeasurementCount() { + return visionValueCount; + } + /** remove algae coral stack obstacles for on the fly */ public void teleopInit() { localADStar.setDynamicObstacles(List.of(), getPose().getTranslation()); @@ -1063,6 +1070,7 @@ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + visionValueCount += 1; poseEstimator.addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); }