diff --git a/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path b/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path index 1f9d069b7c..efd4dbeeaf 100644 --- a/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path +++ b/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path @@ -12,7 +12,7 @@ "y": 3.6901959675661757 }, "isLocked": false, - "linkedName": "Depot" + "linkedName": null }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Outpost" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/L half passing.path b/src/main/deploy/pathplanner/paths/L half passing.path new file mode 100644 index 0000000000..6aad46f794 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L half passing.path @@ -0,0 +1,278 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395203052662038, + "y": 5.71 + }, + "prevControl": null, + "nextControl": { + "x": 4.6936982492178245, + "y": 5.68 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.875901051347584, + "y": 5.71 + }, + "prevControl": { + "x": 5.62851680559387, + "y": 5.67 + }, + "nextControl": { + "x": 6.3981325873247705, + "y": 5.65 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.911440798858774, + "y": 5.973 + }, + "prevControl": { + "x": 6.663044134064404, + "y": 6.023 + }, + "nextControl": { + "x": 7.159837463653143, + "y": 5.872999999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.282938659058486, + "y": 6.1375320970042795 + }, + "prevControl": { + "x": 7.694863849933857, + "y": 6.337532097004279 + }, + "nextControl": { + "x": 9.318696239216477, + "y": 5.837532097004279 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.813423680452932, + "y": 4.714279600570615 + }, + "prevControl": { + "x": 8.861038144372026, + "y": 4.959703443800439 + }, + "nextControl": { + "x": 8.72920125170448, + "y": 4.414279600570614 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.545435092721117, + "y": 4.351997146936273 + }, + "prevControl": { + "x": 7.783197585165418, + "y": 4.274737861496438 + }, + "nextControl": { + "x": 7.294938829716058, + "y": 4.4333941834753885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.545435092721117, + "y": 5.309457917261056 + }, + "prevControl": { + "x": 7.528314099142832, + "y": 5.035522020046176 + }, + "nextControl": { + "x": 7.571312410841653, + "y": 5.723495007132667 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 5.548 + }, + "prevControl": { + "x": 4.113343590533785, + "y": 5.498 + }, + "nextControl": { + "x": 2.6770625147902907, + "y": 5.598 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.643, + "y": 4.882482168334275 + }, + "prevControl": { + "x": 0.6931106826697202, + "y": 4.545268963571586 + }, + "nextControl": { + "x": 0.606252927687199, + "y": 5.129766726437764 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.066024773466543, + "y": 5.548 + }, + "prevControl": { + "x": 1.3954701409262886, + "y": 4.877353118920394 + }, + "nextControl": { + "x": 2.374599790892193, + "y": 5.856617468246593 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.643, + "y": 5.973 + }, + "prevControl": { + "x": 1.5247597440339258, + "y": 5.597718752491559 + }, + "nextControl": { + "x": 0.41296738854974047, + "y": 6.0709030013297545 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 0.643, + "y": 7.133808844507846 + }, + "prevControl": { + "x": 4.058805991440798, + "y": 7.392582025677604 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9381663113006262, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 1.7590618336886934, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.2132196162046975, + "rotationDegrees": 125.00000000000001 + }, + { + "waypointRelativePos": 4.104477611940269, + "rotationDegrees": 20.0 + }, + { + "waypointRelativePos": 5.441364605543682, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 6.426439232409365, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 6.989339019189722, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 7.5053304904050915, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 10.01492537313423, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 7.189736664415911, + "maxWaypointRelativePos": 11.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.8122889939230367, + "maxWaypointRelativePos": 6.2687373396353925, + "constraints": { + "maxVelocity": 1.3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -135.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L quarter.path b/src/main/deploy/pathplanner/paths/L quarter.path index c3adc3820e..11d3ca9479 100644 --- a/src/main/deploy/pathplanner/paths/L quarter.path +++ b/src/main/deploy/pathplanner/paths/L quarter.path @@ -12,7 +12,7 @@ "y": 5.695287264914479 }, "isLocked": false, - "linkedName": null + "linkedName": "L starting" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path index 7534df1f7e..023a9a4779 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path @@ -4,15 +4,15 @@ { "anchor": { "x": 0.643, - "y": 0.736 + "y": 5.973 }, "prevControl": null, "nextControl": { "x": 1.410966233360837, - "y": 1.0471295424792166 + "y": 6.284129542479217 }, "isLocked": false, - "linkedName": null + "linkedName": "Outpost" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path b/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path index fdfa3fbcd2..8d38db9565 100644 --- a/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path +++ b/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path @@ -12,7 +12,7 @@ "y": 3.797670514229443 }, "isLocked": false, - "linkedName": "Outpost" + "linkedName": null }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Depot" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path new file mode 100644 index 0000000000..56c7b506ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -0,0 +1,306 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395203052662038, + "y": 2.5 + }, + "prevControl": null, + "nextControl": { + "x": 4.6936982492178245, + "y": 2.521745650604434 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.875901051347584, + "y": 2.5 + }, + "prevControl": { + "x": 5.62851680559387, + "y": 2.536069862113212 + }, + "nextControl": { + "x": 6.3981325873247705, + "y": 2.423856026334752 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.953260436028501, + "y": 2.125349819934945 + }, + "prevControl": { + "x": 6.704863771234131, + "y": 2.1536181223076314 + }, + "nextControl": { + "x": 7.20165710082287, + "y": 2.097081517562259 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.853981954770756, + "y": 2.125349819934945 + }, + "prevControl": { + "x": 7.2659071456461275, + "y": 2.196210918292518 + }, + "nextControl": { + "x": 8.889739534928747, + "y": 2.000544406366172 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.71051216891264, + "y": 2.954287000464685 + }, + "prevControl": { + "x": 8.749313992967286, + "y": 2.707316519536229 + }, + "nextControl": { + "x": 8.626289740164188, + "y": 3.4903559673172255 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.517431846344486, + "y": 3.7873843614467164 + }, + "prevControl": { + "x": 8.988947103469636, + "y": 4.125038191217473 + }, + "nextControl": { + "x": 7.273764336534804, + "y": 3.7314724247681386 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.798951701905204, + "y": 3.1991433356567542 + }, + "prevControl": { + "x": 6.980324711280639, + "y": 3.371200975621093 + }, + "nextControl": { + "x": 6.617578692529769, + "y": 3.0270856956924153 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.875901051347584, + "y": 2.7329908708952915 + }, + "prevControl": { + "x": 6.520950949811972, + "y": 2.815194654305315 + }, + "nextControl": { + "x": 5.230851152883195, + "y": 2.650787087485268 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 2.7329908708952915 + }, + "prevControl": { + "x": 4.113343590533785, + "y": 2.743221518742256 + }, + "nextControl": { + "x": 2.6770625147902907, + "y": 2.722760223048327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7, + "y": 2.5 + }, + "prevControl": { + "x": 1.0372296339053684, + "y": 2.553445583565675 + }, + "nextControl": { + "x": 0.3627703660946314, + "y": 2.446554416434325 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.066024773466543, + "y": 1.6619142077137554 + }, + "prevControl": { + "x": 1.3954701409262886, + "y": 2.332561088793362 + }, + "nextControl": { + "x": 2.374599790892193, + "y": 1.353296739467163 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7214374709572491, + "y": 1.5211897362918227 + }, + "prevControl": { + "x": 1.6031972149911748, + "y": 1.896470983800263 + }, + "nextControl": { + "x": 0.49140485950698964, + "y": 1.423286734962068 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.066024773466543, + "y": 0.8233406908302368 + }, + "prevControl": { + "x": 0.7577187786816997, + "y": 1.0345586089662913 + }, + "nextControl": { + "x": 3.0562750735749686, + "y": 0.6634708991635694 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.643, + "y": 0.736 + }, + "prevControl": { + "x": 2.0770736330545216, + "y": 0.5890352578996298 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.99, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 2.9180041326992754, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 7.0, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 8.007536514945645, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 9.0009765625, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 10.995612545289848, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5718485729386888, + "maxWaypointRelativePos": 6.448161667547567, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 150.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 8.2, + "y": 3.2 + }, + "rotationOffset": 90.0, + "minWaypointRelativePos": 3.9905688424947137, + "maxWaypointRelativePos": 5.141087473572938, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 8.2, + "y": 2.7 + }, + "rotationOffset": 90.0, + "minWaypointRelativePos": 3.081907373150104, + "maxWaypointRelativePos": 3.884629360465116, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index c104be027c..495585ba4f 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -97,15 +97,15 @@ { "anchor": { "x": 0.643, - "y": 0.736 + "y": 5.973 }, "prevControl": { "x": 1.2326816465669375, - "y": 0.7360000000000001 + "y": 5.973 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost" } ], "rotationTargets": [ diff --git a/src/main/java/frc/RobotManager.java b/src/main/java/frc/RobotManager.java index 6131556262..de0230b761 100644 --- a/src/main/java/frc/RobotManager.java +++ b/src/main/java/frc/RobotManager.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; import frc.utils.GamePeriodUtils; import frc.utils.HubUtil; @@ -49,10 +51,11 @@ public RobotManager() { JoysticksBindings.configureBindings(robot); Threads.setCurrentThreadPriority(true, 10); - robot.getAutonomousChooser().getChooser().onChange((autonomousCommand) -> { this.autonomousCommand = autonomousCommand.get(); }); +// CommandScheduler.getInstance().schedule(new WaitCommand(1).andThen(new InstantCommand(() -> +// )); } @Override diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7669a7f2b4..5f6f2a5c50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -363,6 +363,8 @@ private void configureAuto() { Supplier autonomousResetSubsystemsCommand = () -> getRobotCommander().setState(RobotState.RESET_SUBSYSTEMS); + Supplier autonomousPassingSequenceCommand = () -> getRobotCommander().passSequence(); + getSwerve().configPathPlanner(() -> getPoseEstimator().getEstimatedPose(), (pose) -> {}, getRobotConfig()); this.autonomousChooser = new AutonomousChooser( @@ -373,6 +375,7 @@ private void configureAuto() { autonomousOpenIntakeCommand, autonomousCloseIntakeCommand, autonomousScoringSequenceCommand, + autonomousPassingSequenceCommand, AutonomousConstants.DEFAULT_PATHFINDING_CONSTRAINTS, AutonomousConstants.DEFAULT_IS_NEAR_END_OF_PATH_TOLERANCE, AutonomousConstants.DEFAULT_STUCK_IS_NEAR_END_OF_PATH_TOLERANCE, diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 4fb9e60cb0..d6a7ee3bc9 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -22,7 +22,9 @@ public class AutonomousConstants { public static final double TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS = 4.0; public static final double TIME_TO_WAIT_TO_START_SHOOTING_AFTER_AUTO_START = 2.0; + public static final double TIME_TO_WAIT_TO_START_PASSING_AFTER_AUTO_START = 2.0; public static final double TIME_TO_WAIT_TO_START_WIGGLE_AFTER_PATH_END = 2.0; + public static final double TIME_TO_WAIT_TO_START_SHOOTING_AFTER_PASSING = 3.0; public static final double TIME_BETWEEN_WIGGLES_SECONDS = 0.3; public static final double TIME_TO_WAIT_AT_DEPOT = 2; public static final Rotation2d WIGGLE_RANGE = Rotation2d.fromDegrees(5); diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index c59a219814..029635dc70 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -29,6 +29,7 @@ public static List> getAutoList( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier passingSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -58,6 +59,32 @@ public static List> getAutoList( stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, AllianceSide.DEPOT + ), + getQuarterPassingAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + passingSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.OUTPOST + ), + getQuarterPassingAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + passingSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT ) ); } @@ -130,6 +157,69 @@ private static Supplier getQuarterAuto( ); } + private static Supplier getQuarterPassingAuto( + Robot robot, + Supplier resetSubsystems, + Supplier openIntake, + Supplier closeIntake, + Supplier scoreSequence, + Supplier passingSequence, + PathConstraints pathfindingConstraints, + Pose2d regularIsNearEndOfPathTolerance, + Pose2d stuckIsNearEndOfPathTolerance, + double stuckDebounceSeconds, + AllianceSide startingSide + ) { + return () -> new PathPlannerAutoWrapper( + new ParallelCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + startingSide == AllianceSide.OUTPOST ? PathHelper.PATH_PLANNER_PATHS.get("R half passing") : PathHelper.PATH_PLANNER_PATHS.get("L half passing"), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + .asProxy() + .alongWith(new InstantCommand(() -> hasPathEnded = false)) + .andThen(new InstantCommand(() -> hasPathEnded = true)), + new SequentialCommandGroup( + resetSubsystems.get(), + new ParallelCommandGroup( + new SequentialCommandGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_PASSING_AFTER_AUTO_START), + new ParallelDeadlineGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_SHOOTING_AFTER_PASSING), + passingSequence.get() + ), + scoreSequence.get() + ), + openIntake.get() + .until(() -> hasPathEnded) + .andThen( + new ParallelCommandGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_WIGGLE_AFTER_PATH_END) + .andThen( + robot.getSwerve() + .getCommandsBuilder() + .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) + ) + .asProxy(), + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) + .andThen(closeIntake.get()) + ) + ) + ) + ) + ), + new Pose2d(), + startingSide == AllianceSide.OUTPOST ? "R quarter passing" : "L quarter passing" + ); + } + private static Command getAllianceSideToStartingLineAuto( Robot robot, AllianceSide allianceSide,