From 64b6653acabf4c10d243900d1f3568cf08787e75 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Mon, 31 Mar 2025 17:00:06 -0700 Subject: [PATCH 1/4] first draft- seeded start points might need to be adjusted, but the framework is there? --- src/main/deploy/auto/1PieceBarge.json | 9 ++- src/main/deploy/auto/1PieceLeft.json | 9 ++- src/main/deploy/auto/2PieceLeft.json | 9 ++- src/main/deploy/auto/2PieceRight.json | 9 ++- src/main/deploy/auto/3PieceLeft.json | 9 ++- src/main/deploy/auto/3PieceRight.json | 9 ++- src/main/deploy/auto/4PieceLeft.json | 9 ++- src/main/deploy/auto/ExampleAutoPath.json | 7 +- .../deploy/constants/comp/FeatureFlags.json | 4 +- .../pathplanner/paths/New New New Path.path | 79 +++++++++++++++++++ .../pathplanner/paths/New New Path.path | 10 +-- src/main/java/frc/robot/BuildConstants.java | 12 +-- src/main/java/frc/robot/StrategyManager.java | 20 +++++ .../frc/robot/constants/AutoStrategy.java | 4 + .../frc/robot/constants/VisionConstants.java | 2 + .../frc/robot/subsystems/drive/Drive.java | 8 ++ 16 files changed, 188 insertions(+), 21 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New New New Path.path 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 a33e81e7..3f0e853f 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 dfbb48b8..ff53c168 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 c0c5ee66..f56fcbb4 100644 --- a/src/main/deploy/auto/4PieceLeft.json +++ b/src/main/deploy/auto/4PieceLeft.json @@ -10,5 +10,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 99498676..52c1a601 100644 --- a/src/main/deploy/constants/comp/FeatureFlags.json +++ b/src/main/deploy/constants/comp/FeatureFlags.json @@ -1,7 +1,7 @@ { "runDrive": true, - "runVision": true, - "runScoring": false, + "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 728103f1..021315a7 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 = 75; - public static final String GIT_SHA = "73c21d22d84522e4d964f142a63f61258c98b164"; - public static final String GIT_DATE = "2025-03-28 18:14:42 EDT"; - public static final String GIT_BRANCH = "tune-climb-better"; - public static final String BUILD_DATE = "2025-03-29 14:00:08 EDT"; - public static final long BUILD_UNIX_TIME = 1743271208637L; + public static final int GIT_REVISION = 71; + public static final String GIT_SHA = "106a765371c33e8f927e7fee23e0804e48663ae6"; + public static final String GIT_DATE = "2025-03-29 14:10:22 EDT"; + public static final String GIT_BRANCH = "202-seed-start-position-and-rotation-in-auto"; + public static final String BUILD_DATE = "2025-03-31 19:58:15 EDT"; + public static final long BUILD_UNIX_TIME = 1743465495972L; 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..4d4a49ba 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,21 @@ public void autonomousInit(AutoStrategy strategy) { this.currentCommand = null; this.currentAction = null; + if (!FeatureFlags.synced.getObject().runVision + || drive.getVisionMeasurementCount() < VisionConstants.synced.getObject().safetyThreshold) { + Pose2d startPose = + new Pose2d(strategy.startSeededPositionTranslation, strategy.startSeededPositionRotation); + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Blue) { + startPose = + new Pose2d( + 17.5 - startPose.getX(), + 8 - 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..739dbbac 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 safetyThreshold = 10; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0085f6e4..c58f889d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -121,6 +121,8 @@ public class Drive implements DriveTemplate { private ChassisSpeeds goalSpeeds = new ChassisSpeeds(); + private int visionValueCount = 0; + public ProfiledPIDController angleController = new ProfiledPIDController( JsonConstants.drivetrainConstants.rotationAlignKp, @@ -377,6 +379,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()); @@ -1029,6 +1036,7 @@ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + visionValueCount += 1; poseEstimator.addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } From 42fea260497d1036c2fd6ee83d1b3d57ff2987b9 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Tue, 1 Apr 2025 17:22:06 -0400 Subject: [PATCH 2/4] save --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/StrategyManager.java | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 021315a7..519bad37 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code"; + public static final String MAVEN_NAME = "2025-Robot-Code-1"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 71; - public static final String GIT_SHA = "106a765371c33e8f927e7fee23e0804e48663ae6"; - public static final String GIT_DATE = "2025-03-29 14:10:22 EDT"; + public static final int GIT_REVISION = 72; + public static final String GIT_SHA = "64b6653acabf4c10d243900d1f3568cf08787e75"; + public static final String GIT_DATE = "2025-03-31 20:00:06 EDT"; public static final String GIT_BRANCH = "202-seed-start-position-and-rotation-in-auto"; - public static final String BUILD_DATE = "2025-03-31 19:58:15 EDT"; - public static final long BUILD_UNIX_TIME = 1743465495972L; + public static final String BUILD_DATE = "2025-04-01 17:11:57 EDT"; + public static final long BUILD_UNIX_TIME = 1743541917907L; 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 4d4a49ba..d563ffbf 100644 --- a/src/main/java/frc/robot/StrategyManager.java +++ b/src/main/java/frc/robot/StrategyManager.java @@ -448,8 +448,8 @@ public void autonomousInit(AutoStrategy strategy) { && DriverStation.getAlliance().get() == Alliance.Blue) { startPose = new Pose2d( - 17.5 - startPose.getX(), - 8 - startPose.getY(), + 17.3736 - startPose.getX(), + 7.9248 - startPose.getY(), new Rotation2d(startPose.getRotation().getRadians() - Math.PI / 2)); } drive.setPose(startPose); From af8f0c2470c1fe5db9d3b98bfa8bfa8f4465f806 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 2 Apr 2025 16:58:49 -0400 Subject: [PATCH 3/4] changed! --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/StrategyManager.java | 3 ++- src/main/java/frc/robot/constants/VisionConstants.java | 2 +- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 519bad37..7d1c47ce 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-1"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 72; - public static final String GIT_SHA = "64b6653acabf4c10d243900d1f3568cf08787e75"; - public static final String GIT_DATE = "2025-03-31 20:00:06 EDT"; + public static final int GIT_REVISION = 75; + public static final String GIT_SHA = "298d1a047bfaf25741cef8481b91f444d9aba18c"; + public static final String GIT_DATE = "2025-04-01 17:22:40 EDT"; public static final String GIT_BRANCH = "202-seed-start-position-and-rotation-in-auto"; - public static final String BUILD_DATE = "2025-04-01 17:11:57 EDT"; - public static final long BUILD_UNIX_TIME = 1743541917907L; + public static final String BUILD_DATE = "2025-04-02 16:57:32 EDT"; + public static final long BUILD_UNIX_TIME = 1743627452138L; 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 d563ffbf..20de1b23 100644 --- a/src/main/java/frc/robot/StrategyManager.java +++ b/src/main/java/frc/robot/StrategyManager.java @@ -441,7 +441,8 @@ public void autonomousInit(AutoStrategy strategy) { this.currentAction = null; if (!FeatureFlags.synced.getObject().runVision - || drive.getVisionMeasurementCount() < VisionConstants.synced.getObject().safetyThreshold) { + || drive.getVisionMeasurementCount() + < VisionConstants.synced.getObject().minimumVisionMeasurementInitCount) { Pose2d startPose = new Pose2d(strategy.startSeededPositionTranslation, strategy.startSeededPositionRotation); if (DriverStation.getAlliance().isPresent() diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 739dbbac..819b3a01 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -25,5 +25,5 @@ public class VisionConstants { Units.inchesToMeters(12.0), new Rotation3d(0, 0, 0)); - public final Integer safetyThreshold = 10; + public final Integer minimumVisionMeasurementInitCount = 10; } From 7e2526232ed1670a12322134eaf2b7052b24c3ff Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 2 Apr 2025 15:15:21 -0700 Subject: [PATCH 4/4] formatting? --- src/main/deploy/constants/comp/FeatureFlags.json | 3 --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/constants/comp/FeatureFlags.json b/src/main/deploy/constants/comp/FeatureFlags.json index 9b1cf6c9..52c1a601 100644 --- a/src/main/deploy/constants/comp/FeatureFlags.json +++ b/src/main/deploy/constants/comp/FeatureFlags.json @@ -3,8 +3,5 @@ "runVision": false, "runScoring": true, "runClimb": true, - "runVision": true, - "runScoring": true, - "runClimb": false, "runRamp": true } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7d1c47ce..f9869f8f 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-1"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 75; - public static final String GIT_SHA = "298d1a047bfaf25741cef8481b91f444d9aba18c"; - public static final String GIT_DATE = "2025-04-01 17:22:40 EDT"; + public static final int GIT_REVISION = 80; + public static final String GIT_SHA = "47ddb4f9278f869403669d4e05da4c1692025fb4"; + public static final String GIT_DATE = "2025-04-02 16:58:57 EDT"; public static final String GIT_BRANCH = "202-seed-start-position-and-rotation-in-auto"; - public static final String BUILD_DATE = "2025-04-02 16:57:32 EDT"; - public static final long BUILD_UNIX_TIME = 1743627452138L; + public static final String BUILD_DATE = "2025-04-02 18:13:49 EDT"; + public static final long BUILD_UNIX_TIME = 1743632029779L; public static final int DIRTY = 1; private BuildConstants() {}