From 02b182480f829f0702bc27697fee0d3d2f6ff270 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 20:03:37 -0700 Subject: [PATCH 1/2] Add timing during practice --- src/main/java/frc/robot/RobotState.java | 1 - src/main/java/frc/robot/util/HubShiftUtil.java | 5 ++++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 3aa69f8..bd5fcb4 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -70,7 +70,6 @@ public static void periodic() { inAllianceZone = currentPose.getMeasureX().minus(allianceWall).abs(Meters) < ShooterConstants.Positions.spinUpZone.in(Meters); // Other logging - Logger.recordOutput("ShiftInfo", HubShiftUtil.getOfficialShiftInfo()); LoggedTracer.record("RobotState"); diff --git a/src/main/java/frc/robot/util/HubShiftUtil.java b/src/main/java/frc/robot/util/HubShiftUtil.java index 7fffd63..63b6f85 100644 --- a/src/main/java/frc/robot/util/HubShiftUtil.java +++ b/src/main/java/frc/robot/util/HubShiftUtil.java @@ -57,6 +57,8 @@ public record ShiftInfo( private static final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; private static final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + private static final boolean useTimingsAtHome = true; + // private static final double minFuelCountDelay = 1.0; // private static final double maxFuelCountDelay = 2.0; // private static final double shiftEndFuelCountExtension = 3.0; @@ -139,7 +141,8 @@ private static ShiftInfo getShiftInfo( // Adjust the current offset if the time difference above the theshold if (Math.abs(fieldTeleopTime - currentTime) >= timeResetThreshold && fieldTeleopTime <= 135 - && DriverStation.isFMSAttached()) { + && (DriverStation.isFMSAttached() || useTimingsAtHome) + ) { // shiftTimerOffset += currentTime - fieldTeleopTime; // currentTime = timerValue + shiftTimerOffset; From ebbffe1fe1cc0c12ce691aeae923b94433a0fc39 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 20:59:45 -0700 Subject: [PATCH 2/2] Add aarush auto --- ...tLoop2(loopie).path => a7CollectLoop.path} | 102 ++++++++++------- .../pathplanner/paths/a7DepotCollectFast.path | 107 ++++++++++++++++++ .../pathplanner/paths/a7StartToCollect.path | 54 +++++++++ src/main/deploy/pathplanner/settings.json | 3 +- src/main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/commands/AutoCommands.java | 30 +++++ 6 files changed, 258 insertions(+), 43 deletions(-) rename src/main/deploy/pathplanner/paths/{a5CollectLoop2(loopie).path => a7CollectLoop.path} (55%) create mode 100644 src/main/deploy/pathplanner/paths/a7DepotCollectFast.path create mode 100644 src/main/deploy/pathplanner/paths/a7StartToCollect.path diff --git a/src/main/deploy/pathplanner/paths/a5CollectLoop2(loopie).path b/src/main/deploy/pathplanner/paths/a7CollectLoop.path similarity index 55% rename from src/main/deploy/pathplanner/paths/a5CollectLoop2(loopie).path rename to src/main/deploy/pathplanner/paths/a7CollectLoop.path index 46b9a0d..805fe79 100644 --- a/src/main/deploy/pathplanner/paths/a5CollectLoop2(loopie).path +++ b/src/main/deploy/pathplanner/paths/a7CollectLoop.path @@ -8,56 +8,72 @@ }, "prevControl": null, "nextControl": { - "x": 3.9234084117797794, - "y": 5.258754148807581 + "x": 3.362907698818017, + "y": 5.328997170250767 }, "isLocked": false, "linkedName": "a5LoopShoot" }, { "anchor": { - "x": 7.792755323561947, + "x": 6.708995782632744, "y": 5.368689988938053 }, "prevControl": { - "x": 7.373079525779589, - "y": 6.539593022210406 + "x": 5.36582152527449, + "y": 6.343308618284048 }, "nextControl": { - "x": 8.004585274377915, - "y": 4.777680611559139 + "x": 7.359081995435712, + "y": 4.896981921121975 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.970961939988938, - "y": 4.465526721515486 + "x": 6.708995782632744, + "y": 2.9864684043141585 }, "prevControl": { - "x": 8.193042254437715, - "y": 3.511669505878289 + "x": 7.697838255014892, + "y": 2.580282702370341 }, "nextControl": { - "x": 6.101025719026548, - "y": 5.144528691924779 + "x": 5.825374028251257, + "y": 3.349432711148226 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.681115182522125, - "y": 5.668036573561946 + "x": 6.129280835176992, + "y": 4.461494607300884 }, "prevControl": { - "x": 5.639121266592921, - "y": 5.64160045630531 + "x": 6.156139549466578, + "y": 4.0372859451716305 }, "nextControl": { - "x": 3.728392890984115, - "y": 5.694326884898057 + "x": 6.1024221208874065, + "y": 4.885703269430138 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.654466848727876, + "y": 5.592790652654867 + }, + "prevControl": { + "x": 5.071713567647901, + "y": 5.52944676656619 + }, + "nextControl": { + "x": 4.067828395426444, + "y": 5.681850572544342 }, "isLocked": false, "linkedName": null @@ -68,12 +84,12 @@ "y": 5.368689988938053 }, "prevControl": { - "x": 2.8778707569951325, - "y": 5.292815724664674 + "x": 2.8839161907194075, + "y": 5.275951112520585 }, "nextControl": null, "isLocked": false, - "linkedName": "a5LoopShoot" + "linkedName": null } ], "rotationTargets": [ @@ -81,20 +97,24 @@ "waypointRelativePos": 0.5972188995215311, "rotationDegrees": -22.77253519883315 }, + { + "waypointRelativePos": 1.0519386574074077, + "rotationDegrees": -56.98406371610071 + }, { "waypointRelativePos": 1.7967115097160609, - "rotationDegrees": -102.12464780763888 + "rotationDegrees": 165.52779753636696 }, { - "waypointRelativePos": 2.5, - "rotationDegrees": -38.095099380259605 + "waypointRelativePos": 3.0, + "rotationDegrees": 134.27333953270954 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.5812305900621126, - "maxWaypointRelativePos": 3.46074340062112, + "minWaypointRelativePos": 3.15, + "maxWaypointRelativePos": 4.46074340062112, "constraints": { "maxVelocity": 2.0, "maxAcceleration": 3.0, @@ -118,19 +138,21 @@ } } ], - "pointTowardsZones": [ + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "startintake", + "waypointRelativePos": 0.6701276077291383, + "endWaypointRelativePos": null, + "command": null + }, { - "fieldPosition": { - "x": 4.5974, - "y": 4.035 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 3.339334239130434, - "maxWaypointRelativePos": 4.0, - "name": "Point Towards Zone" + "name": "stopintake", + "waypointRelativePos": 3.584024452804379, + "endWaypointRelativePos": null, + "command": null } ], - "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, "maxAcceleration": 3.0, @@ -141,13 +163,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -41.428979515269766 + "rotation": -41.79210441429713 }, "reversed": false, - "folder": "T2NZ2TSameAlt", + "folder": "CooperativeDepot", "idealStartingState": { "velocity": 0, - "rotation": -41.428979515269766 + "rotation": -41.42897951526977 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path b/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path new file mode 100644 index 0000000..dc95a05 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.682, + "y": 7.15 + }, + "prevControl": null, + "nextControl": { + "x": 0.6820000000000002, + "y": 6.15 + }, + "isLocked": false, + "linkedName": "CollectDepotStart" + }, + { + "anchor": { + "x": 0.668, + "y": 5.6 + }, + "prevControl": { + "x": 0.668, + "y": 6.209319597164331 + }, + "nextControl": { + "x": 0.668, + "y": 4.990680402835668 + }, + "isLocked": false, + "linkedName": "CollectDepotInner" + }, + { + "anchor": { + "x": 3.116, + "y": 5.369 + }, + "prevControl": { + "x": 2.1735160398230096, + "y": 4.853859513274335 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "a5LoopShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -119.99999999999999 + } + ], + "constraintZones": [ + { + "name": "Intake", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.800786593707251, + "constraints": { + "maxVelocity": 0.8, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.5974, + "y": 4.034536 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.7439722982216141, + "maxWaypointRelativePos": 2.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [ + { + "name": "stopintake", + "waypointRelativePos": 1.4608947503419971, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -41.42897951526977 + }, + "reversed": false, + "folder": "CooperativeDepot", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/a7StartToCollect.path b/src/main/deploy/pathplanner/paths/a7StartToCollect.path new file mode 100644 index 0000000..d6df2e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/a7StartToCollect.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6584133711283187, + "y": 6.005278968473451 + }, + "prevControl": null, + "nextControl": { + "x": 2.2699642563357614, + "y": 6.63784165139431 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.682, + "y": 7.15 + }, + "prevControl": { + "x": 1.6802532288177803, + "y": 6.773093005718346 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CollectDepotStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "CooperativeDepot", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index bd4fb46..fb03832 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,11 +4,12 @@ "holonomicMode": true, "pathFolders": [ "DepotAuto", - "Trench NZ Depot", + "CooperativeDepot", "PreloadOnly", "T2NZ2TSame", "T2NZ2TSameAlt", "TestCollect", + "Trench NZ Depot", "Trench2Trench" ], "autoFolders": [], diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87bb2bd..09a39b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -249,6 +249,7 @@ public RobotContainer() { autoChooser_ = new LoggedDashboardChooser<>("Auto Choices"); autoChooser_.addDefaultOption("Neutral Zone Loop W/ Depot", AutoCommands.a6CollectNZDepot(drivebase_, shooter_, intake_, hopper_)); + autoChooser_.addOption("Cooperative Depot Auto", AutoCommands.a7CoopDepot(drivebase_, shooter_, intake_, hopper_)); autoChooser_.addOption("Neutral Zone Loop - Left Trench", AutoCommands.a5CollectNZ(drivebase_, shooter_, intake_, hopper_, false)); autoChooser_.addOption("Neutral Zone Loop - Right Trench", AutoCommands.a5CollectNZ(drivebase_, shooter_, intake_, hopper_, true)); autoChooser_.addOption("Preload Only (untested)", AutoCommands.a4OnlyPreload(drivebase_, shooter_, intake_, hopper_)); @@ -304,8 +305,8 @@ private void configureBindings() { // When the shooter isnt shooting, get it ready to shoot. shooter_.setDefaultCommand(shooter_.spinUpVelocityCommand(() -> { - var shift = HubShiftUtil.getOfficialShiftInfo(); - return shift.active() ? RotationsPerSecond.of(50) : RotationsPerSecond.zero(); + return RotationsPerSecond.zero(); + // return HubShiftUtil.getOfficialShiftInfo().active() ? RotationsPerSecond.of(50) : RotationsPerSecond.zero(); })); //While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index f0a218d..8e012ec 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -149,4 +149,34 @@ public static Command a6CollectNZDepot(Drive drive, Shooter shooter, IntakeSubsy RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(shootingLength2) ); } + + public static Command a7CoopDepot(Drive drive, Shooter shooter, IntakeSubsystem intake, Hopper hopper) { + var startIntake = new EventTrigger("startintake"); + var stopIntake = new EventTrigger("stopintake"); + + return Commands.sequence( + DriveCommands.initialFollowPathCommand(drive, "a7StartToCollect") + .deadlineFor( + new StartupCmd(intake, hopper, shooter) + .withTimeout(Seconds.of(1.5)) + ), + + DriveCommands.followPathCommand("a7DepotCollectFast").deadlineFor( + RobotCommands.intake(intake, hopper) + .until(stopIntake) + .andThen(shooter.spinUpForDistanceHoodParked(() -> Meters.of(2))) + ), + + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5)), + + DriveCommands.followPathCommand("a7CollectLoop").deadlineFor( + RobotCommands.intake(intake, hopper) + .beforeStarting(Commands.waitUntil(startIntake)) + .until(stopIntake) + .andThen(shooter.spinUpForDistanceHoodParked(() -> Meters.of(2))) + ), + + RobotCommands.shoot(shooter, hopper, intake, drive) + ); + } } \ No newline at end of file