Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add missing paths for automation #191

Merged
merged 3 commits into from
Apr 18, 2024
Merged

add missing paths for automation #191

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
18a928d
add source path and driveToSource method
linglejack06 Apr 16, 2024
29b7166
remove duplicate code in favor of method
linglejack06 Apr 16, 2024
33a7998
add centerline + speaker path
TheLumpiest Apr 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .pathplanner/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@
"robotLength": 0.9525,
"holonomicMode": true,
"pathFolders": [
"Endgame"
"Endgame",
"Source"
],
"autoFolders": [],
"defaultMaxVel": 4.0,
Expand Down
55 changes: 55 additions & 0 deletions src/main/deploy/pathplanner/paths/rightSource.path
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 5.432455267688313,
"y": 1.0401173811606885
},
"prevControl": null,
"nextControl": {
"x": 4.699058199042324,
"y": 1.5593743826376547
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.3367740994741248,
"y": 1.6330703957704211
},
"prevControl": {
"x": 1.5714611614408416,
"y": 2.8244991721464925
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.5,
"rotationDegrees": -49.42789973843088,
"rotateFast": false
}
],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 12.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": -118.51259584088376,
"rotateFast": false
},
"reversed": false,
"folder": "Source",
"previewStartingState": null,
"useDefaultConstraints": false
}
49 changes: 49 additions & 0 deletions src/main/deploy/pathplanner/paths/shopSource.path
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 6.965564788272434,
"y": 6.4355020138611705
},
"prevControl": null,
"nextControl": {
"x": 7.595921477557241,
"y": 6.391523640190138
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 8.29957545629377,
"y": 6.801988461119779
},
"prevControl": {
"x": 7.874451177473783,
"y": 6.582096592764614
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 12.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": -150.461217740442,
"rotateFast": false
},
"reversed": false,
"folder": "Source",
"previewStartingState": null,
"useDefaultConstraints": false
}
52 changes: 52 additions & 0 deletions src/main/deploy/pathplanner/paths/speaker.path
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 8.226278166842048,
"y": 6.420842555970826
},
"prevControl": null,
"nextControl": {
"x": 6.24725135164556,
"y": 8.179977502812148
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.6588410319677789,
"y": 5.541275082550166
},
"prevControl": {
"x": 0.6588410319677789,
"y": 5.541275082550166
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 12.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": -177.13759477388828,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": -135.88140399658215,
"velocity": 0
},
"useDefaultConstraints": true
}
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,9 @@ public static final class FeatureFlags {
public static final boolean runVision = true;
public static final boolean runLocalizer = true;

public static final boolean runIntake = true;
public static final boolean runScoring = true;
public static final boolean runEndgame = true;
public static final boolean runIntake = false;
public static final boolean runScoring = false;
public static final boolean runEndgame = false;
public static final boolean runDrive = true;

public static final boolean enableLEDS = true;
Expand Down
12 changes: 8 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -329,9 +329,13 @@ private void configureBindings() {
.onTrue(new InstantCommand(
() -> drivetrain.setAlignTarget(AlignTarget.RIGHT)));

// controller.x()
// .onTrue(new InstantCommand(() -> drivetrain.driveToEndgame()))
// .onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
controller.x()
.onTrue(new InstantCommand(() -> drivetrain.driveToSource()))
.onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));

controller.y()
.onTrue(new InstantCommand(() -> drivetrain.driveToSpeaker()))
.onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
}

if (FeatureFlags.runEndgame) {
Expand Down Expand Up @@ -935,7 +939,7 @@ public void disabledPeriodic() {
drivetrain.setBrakeMode(true);
}
}
if (ledSwitch != null) {
if (ledSwitch != null && leds != null) {
leds.setEnabled(!ledSwitch.get());
}
}
Expand Down
92 changes: 33 additions & 59 deletions src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import com.pathplanner.lib.pathfinding.Pathfinding;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.PathPlannerLogging;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
Expand Down Expand Up @@ -550,6 +551,31 @@ public void driveToPose(Pose2d targetPose) {
pathfindCommand.schedule();
}

public void driveToPath(String pathName) {
this.setAlignState(AlignState.MANUAL);

PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);

// if(DriverStation.getAlliance() === Alliance.Blue) {
// path.flipPath();
// }

PathConstraints constraints =
new PathConstraints(
3.0,
4.0,
Constants.ConversionConstants.kDegreesToRadians * 540,
Constants.ConversionConstants.kDegreesToRadians * 720);

PathPlannerLogging.setLogTargetPoseCallback(
(target) -> {
Logger.recordOutput("targetPose", target);
});

pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0);
pathfindCommand.schedule();
}

public void stopDriveToPose() {
if (pathfindCommand != null) {
pathfindCommand.cancel();
Expand All @@ -562,50 +588,12 @@ public void setPoseTarget(Pose2d pose) {
targetTightPose = pose;
}

public Pose2d getEndgamePose() {
// Blue Alliance Poses
Pose2d leftClimbPose2d = new Pose2d(4.61, 4.48, Rotation2d.fromDegrees(-60));
Pose2d rightClimbPose2d = new Pose2d(4.66, 3.67, Rotation2d.fromDegrees(60));
Pose2d farClimbPose2d = new Pose2d(5.38, 4.11, Rotation2d.fromDegrees(180));

if (DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
// Red Alliance Poses
leftClimbPose2d = new Pose2d(11.882, 3.67, Rotation2d.fromDegrees(120));
rightClimbPose2d = new Pose2d(11.932, 4.48, Rotation2d.fromDegrees(-120));
farClimbPose2d = new Pose2d(11.162, 4.11, Rotation2d.fromDegrees(0));
}

double distanceToTargetLeft =
Math.hypot(
getFieldToRobot.get().getX() - leftClimbPose2d.getX(),
getFieldToRobot.get().getY() - leftClimbPose2d.getY());
double distanceToTargetRight =
Math.hypot(
getFieldToRobot.get().getX() - rightClimbPose2d.getX(),
getFieldToRobot.get().getY() - rightClimbPose2d.getY());
double distanceToTargetFar =
Math.hypot(
getFieldToRobot.get().getX() - farClimbPose2d.getX(),
getFieldToRobot.get().getY() - farClimbPose2d.getY());

Pose2d targetPose = new Pose2d(new Translation2d(), Rotation2d.fromDegrees(180));

if (distanceToTargetLeft < distanceToTargetRight
&& distanceToTargetLeft < distanceToTargetFar) {
targetPose = leftClimbPose2d;
} else if (distanceToTargetRight < distanceToTargetLeft
&& distanceToTargetRight < distanceToTargetFar) {
targetPose = rightClimbPose2d;
} else {
targetPose = farClimbPose2d;
}

return targetPose;
public void driveToSpeaker() {
driveToPath("speaker");
}

public void driveToSpeaker() {
driveToPose(new Pose2d(AllianceUtil.getFieldToSpeaker(), new Rotation2d()));
public void driveToSource() {
driveToPath("shopSource");
}

public void driveToEndgame() {
Expand Down Expand Up @@ -635,29 +623,15 @@ public void driveToEndgame() {
getFieldToRobot.get().getX() - farClimbPose2d.getX(),
getFieldToRobot.get().getY() - farClimbPose2d.getY());

PathPlannerPath path = null;

if (distanceToTargetLeft < distanceToTargetRight
&& distanceToTargetLeft < distanceToTargetFar) {
path = PathPlannerPath.fromPathFile("LeftEndgame");
driveToPath("leftEndgame");
} else if (distanceToTargetRight < distanceToTargetLeft
&& distanceToTargetRight < distanceToTargetFar) {
path = PathPlannerPath.fromPathFile("RightEndgame");
driveToPath("rightEndgame");
} else {
path = PathPlannerPath.fromPathFile("FarEndgame");
driveToPath("farEndgame");
}

this.setAlignState(AlignState.MANUAL);

PathConstraints constraints =
new PathConstraints(
3.0,
4.0,
Constants.ConversionConstants.kDegreesToRadians * 540,
Constants.ConversionConstants.kDegreesToRadians * 720);

pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0);
pathfindCommand.schedule();
}

public boolean isAligned() {
Expand Down
Loading