Skip to content

Commit

Permalink
path planner
Browse files Browse the repository at this point in the history
  • Loading branch information
@Oliver-Cushman
Oliver-Cushman committed Oct 23, 2024
1 parent 7fe0a08 commit da37ea3
Show file tree
Hide file tree
Showing 11 changed files with 164 additions and 30 deletions.
76 changes: 76 additions & 0 deletions src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto
View file
Original file line number Diff line number Diff line change
@@ -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
}
10 changes: 5 additions & 5 deletions src/main/deploy/pathplanner/paths/C3 M.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -55,7 +55,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 174.07710833256513,
"rotation": 177.03906386583628,
"rotateFast": false
},
"reversed": false,
Expand Down
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/C4 M.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/M C2.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/M C3.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/M C4.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/M W3.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
8 changes: 4 additions & 4 deletions src/main/deploy/pathplanner/paths/Shot Spots.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/util/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
57 changes: 57 additions & 0 deletions src/main/java/frc/robot/util/auto/PathPlannerStorage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit da37ea3

Please sign in to comment.