diff --git a/src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto b/src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto new file mode 100644 index 00000000..9d5c394f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2422530556147342, + "y": 7.161301131112901 + }, + "rotation": -124.91237183543709 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload2" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S C1" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C1toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C3 M.path b/src/main/deploy/pathplanner/paths/C3 M.path index ae4ea687..683097b5 100644 --- a/src/main/deploy/pathplanner/paths/C3 M.path +++ b/src/main/deploy/pathplanner/paths/C3 M.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": { - "x": 4.543668034704492, - "y": 4.530204775228513 + "x": 4.772671935242799, + "y": 4.554072325635369 }, "nextControl": null, "isLocked": false, @@ -55,7 +55,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 174.07710833256513, + "rotation": 177.03906386583628, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C4 M.path b/src/main/deploy/pathplanner/paths/C4 M.path index c77d2409..d5ba38be 100644 --- a/src/main/deploy/pathplanner/paths/C4 M.path +++ b/src/main/deploy/pathplanner/paths/C4 M.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": { - "x": 4.400411368945171, - "y": 4.2735685023441725 + "x": 4.629415269483479, + "y": 4.297436052751029 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/M C2.path b/src/main/deploy/pathplanner/paths/M C2.path index bdb5e85f..bb10b2d7 100644 --- a/src/main/deploy/pathplanner/paths/M C2.path +++ b/src/main/deploy/pathplanner/paths/M C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 5.066516560804678, - "y": 7.345159953477801 + "x": 5.295520461342985, + "y": 7.3690275038846575 }, "isLocked": false, "linkedName": "M" diff --git a/src/main/deploy/pathplanner/paths/M C3.path b/src/main/deploy/pathplanner/paths/M C3.path index 96c2f66f..2c8d3e36 100644 --- a/src/main/deploy/pathplanner/paths/M C3.path +++ b/src/main/deploy/pathplanner/paths/M C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 4.965394208503983, - "y": 3.8104740935125765 + "x": 5.1943981090422895, + "y": 3.8343416439194327 }, "isLocked": false, "linkedName": "M" diff --git a/src/main/deploy/pathplanner/paths/M C4.path b/src/main/deploy/pathplanner/paths/M C4.path index 500e0646..dd221049 100644 --- a/src/main/deploy/pathplanner/paths/M C4.path +++ b/src/main/deploy/pathplanner/paths/M C4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 5.664057733490608, - "y": 4.348261148929913 + "x": 5.893061634028916, + "y": 4.372128699336769 }, "isLocked": false, "linkedName": "M" diff --git a/src/main/deploy/pathplanner/paths/M W3.path b/src/main/deploy/pathplanner/paths/M W3.path index 2bd0783e..32481f3c 100644 --- a/src/main/deploy/pathplanner/paths/M W3.path +++ b/src/main/deploy/pathplanner/paths/M W3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 2.5614401060829, - "y": 4.904434086583739 + "x": 2.790444006621207, + "y": 4.928301636990595 }, "isLocked": false, "linkedName": "M" diff --git a/src/main/deploy/pathplanner/paths/Shot Spots.path b/src/main/deploy/pathplanner/paths/Shot Spots.path index 5e89920d..e02f9c98 100644 --- a/src/main/deploy/pathplanner/paths/Shot Spots.path +++ b/src/main/deploy/pathplanner/paths/Shot Spots.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": { - "x": 2.954170685497026, - "y": 5.046924673916537 + "x": 3.1831745860353333, + "y": 5.070792224323394 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 752329b7..2a8d582f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -901,6 +901,7 @@ private void registerPathToPathCommands() { } NamedCommands.registerCommand("C" + i + "toC" + j, pathPlannerStorage.generateCenterLogicOBJ(i, j, swerve, limelight3)); NamedCommands.registerCommand("C" + i + "toC" + j + "nonOBJ", pathPlannerStorage.generateCenterLogicNonOBJ(i, j, swerve)); + NamedCommands.registerCommand("C" + i + "toC" + j + "default", pathPlannerStorage.generateCenterLogicDefault(i, j, swerve)); } } } diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index b5ddf4b0..b880b8fa 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -506,7 +506,7 @@ public static final class AutoConstants { "S W3-1 S C3-5 S", "S W3-1 S C1-5 S", "S W3-1 S", - "S C1-5 S Over W1", + "S C1-5 S Over W1 DEF", "S W1 S C1-5 S", // UNTESTED "S W2 S C3-5 S", // UNTESTED "S W2 S C1-5 S", // UNTESTED diff --git a/src/main/java/frc/robot/util/auto/PathPlannerStorage.java b/src/main/java/frc/robot/util/auto/PathPlannerStorage.java index 86c61328..a6592de1 100644 --- a/src/main/java/frc/robot/util/auto/PathPlannerStorage.java +++ b/src/main/java/frc/robot/util/auto/PathPlannerStorage.java @@ -197,6 +197,18 @@ public Command generateCenterLogicOBJ(int startingNote, int endingNote, Swerve s return commandGroup; } + public Command generateCenterLogicDefault(int startingNote, int endingNote, Swerve swerve) { + SequentialCommandGroup commandGroup = new SequentialCommandGroup(); + boolean goingDown = startingNote < endingNote; + int increment = goingDown ? 1 : -1; + + for (int i = startingNote; (goingDown && i <= endingNote) || (!goingDown && i >= endingNote); i += increment) { + commandGroup.addCommands(generateCenterDefaultCommand(i, endingNote, goingDown, increment, commandGroup)); + } + + return commandGroup; + } + /** * Generates a command for the object detection scenario. */ @@ -309,6 +321,51 @@ private Command generateNonObjectDetectionCommand(int i, int endingNote, boolean commandGroup.getRequirements())); } + private Command generateCenterDefaultCommand(int i, int endingNote, boolean goingDown, int increment, SequentialCommandGroup commandGroup) { + String shootingLocation = (i < 3) ? "L" : (i == 3) ? "M" : "R"; + PathPlannerPath shootNote = PathPlannerPath.fromPathFile("C" + i + " " + shootingLocation); + + if (i == FieldConstants.CENTER_NOTE_COUNT && goingDown || i == 1 && !goingDown || i == endingNote) { + return Commands.defer(() -> + Commands.sequence( + Commands.deadline( + AutoBuilder.followPath(shootNote), + NamedCommands.getCommand("StopIntake") + .andThen(NamedCommands.getCommand("ToIndexer") + .onlyIf(() -> !shooterSensor.getAsBoolean())), + NamedCommands.getCommand("PrepareSWD") + ), + NamedCommands.getCommand("ShootInstantlyWhenReady") + ).onlyIf(() -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean()), + commandGroup.getRequirements()); + } + + PathPlannerPath getNoteAfterShot = PathPlannerPath.fromPathFile(shootingLocation + " C" + (i + increment)); + + Command shootAndMoveToNextNote = + Commands.sequence( + Commands.deadline( + AutoBuilder.followPath(shootNote) + .raceWith(Commands.waitUntil(() -> !shooterSensor.getAsBoolean() && !elevatorSensor.getAsBoolean() && swerve.insideOwnWing())), + NamedCommands.getCommand("StopIntake") + .andThen(NamedCommands.getCommand("ToIndexer") + .onlyIf(() -> !shooterSensor.getAsBoolean())), + NamedCommands.getCommand("PrepareSWD") + ), + NamedCommands.getCommand("ShootInstantlyWhenReady"), + Commands.deadline( + AutoBuilder.followPath(getNoteAfterShot), + Commands.sequence( + NamedCommands.getCommand("StopAll"), + Commands.waitSeconds(1), + NamedCommands.getCommand("ToIndexer") + ) + ) + ); + + return Commands.defer(() -> shootAndMoveToNextNote, commandGroup.getRequirements()); + } + /** * Uses Pathplanner's pathfinding algorithm to go to the closest shooting position * from the swerve subsystem's current position