Skip to content

Commit 3990770

Browse files
linglejack06linglejack06
authored
add missing paths for automation (#191)
1 parent 14f6a9b commit 3990770

File tree

7 files changed

+202
-67
lines changed

7 files changed

+202
-67
lines changed

‎.pathplanner/settings.json‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,8 @@
33
"robotLength": 0.9525,
44
"holonomicMode": true,
55
"pathFolders": [
6-
"Endgame"
6+
"Endgame",
7+
"Source"
78
],
89
"autoFolders": [],
910
"defaultMaxVel": 4.0,

‎src/main/deploy/pathplanner/paths/rightSource.path‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 5.432455267688313,
7+
"y": 1.0401173811606885
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 4.699058199042324,
12+
"y": 1.5593743826376547
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 1.3367740994741248,
20+
"y": 1.6330703957704211
21+
},
22+
"prevControl": {
23+
"x": 1.5714611614408416,
24+
"y": 2.8244991721464925
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [
32+
{
33+
"waypointRelativePos": 0.5,
34+
"rotationDegrees": -49.42789973843088,
35+
"rotateFast": false
36+
}
37+
],
38+
"constraintZones": [],
39+
"eventMarkers": [],
40+
"globalConstraints": {
41+
"maxVelocity": 4.0,
42+
"maxAcceleration": 12.0,
43+
"maxAngularVelocity": 540.0,
44+
"maxAngularAcceleration": 720.0
45+
},
46+
"goalEndState": {
47+
"velocity": 0,
48+
"rotation": -118.51259584088376,
49+
"rotateFast": false
50+
},
51+
"reversed": false,
52+
"folder": "Source",
53+
"previewStartingState": null,
54+
"useDefaultConstraints": false
55+
}

‎src/main/deploy/pathplanner/paths/shopSource.path‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 6.965564788272434,
7+
"y": 6.4355020138611705
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 7.595921477557241,
12+
"y": 6.391523640190138
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 8.29957545629377,
20+
"y": 6.801988461119779
21+
},
22+
"prevControl": {
23+
"x": 7.874451177473783,
24+
"y": 6.582096592764614
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"eventMarkers": [],
34+
"globalConstraints": {
35+
"maxVelocity": 4.0,
36+
"maxAcceleration": 12.0,
37+
"maxAngularVelocity": 540.0,
38+
"maxAngularAcceleration": 720.0
39+
},
40+
"goalEndState": {
41+
"velocity": 0,
42+
"rotation": -150.461217740442,
43+
"rotateFast": false
44+
},
45+
"reversed": false,
46+
"folder": "Source",
47+
"previewStartingState": null,
48+
"useDefaultConstraints": false
49+
}

‎src/main/deploy/pathplanner/paths/speaker.path‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 8.226278166842048,
7+
"y": 6.420842555970826
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 6.24725135164556,
12+
"y": 8.179977502812148
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 1.6588410319677789,
20+
"y": 5.541275082550166
21+
},
22+
"prevControl": {
23+
"x": 0.6588410319677789,
24+
"y": 5.541275082550166
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"eventMarkers": [],
34+
"globalConstraints": {
35+
"maxVelocity": 4.0,
36+
"maxAcceleration": 12.0,
37+
"maxAngularVelocity": 540.0,
38+
"maxAngularAcceleration": 720.0
39+
},
40+
"goalEndState": {
41+
"velocity": 0,
42+
"rotation": -177.13759477388828,
43+
"rotateFast": false
44+
},
45+
"reversed": false,
46+
"folder": null,
47+
"previewStartingState": {
48+
"rotation": -135.88140399658215,
49+
"velocity": 0
50+
},
51+
"useDefaultConstraints": true
52+
}

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,9 @@ public static final class FeatureFlags {
5252
public static final boolean runVision = true;
5353
public static final boolean runLocalizer = true;
5454

55-
public static final boolean runIntake = true;
56-
public static final boolean runScoring = true;
57-
public static final boolean runEndgame = true;
55+
public static final boolean runIntake = false;
56+
public static final boolean runScoring = false;
57+
public static final boolean runEndgame = false;
5858
public static final boolean runDrive = true;
5959

6060
public static final boolean enableLEDS = true;

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -329,9 +329,13 @@ private void configureBindings() {
329329
.onTrue(new InstantCommand(
330330
() -> drivetrain.setAlignTarget(AlignTarget.RIGHT)));
331331

332-
// controller.x()
333-
// .onTrue(new InstantCommand(() -> drivetrain.driveToEndgame()))
334-
// .onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
332+
controller.x()
333+
.onTrue(new InstantCommand(() -> drivetrain.driveToSource()))
334+
.onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
335+
336+
controller.y()
337+
.onTrue(new InstantCommand(() -> drivetrain.driveToSpeaker()))
338+
.onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
335339
}
336340

337341
if (FeatureFlags.runEndgame) {
@@ -935,7 +939,7 @@ public void disabledPeriodic() {
935939
drivetrain.setBrakeMode(true);
936940
}
937941
}
938-
if (ledSwitch != null) {
942+
if (ledSwitch != null && leds != null) {
939943
leds.setEnabled(!ledSwitch.get());
940944
}
941945
}

‎src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java‎

Lines changed: 33 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import com.pathplanner.lib.pathfinding.Pathfinding;
1717
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
1818
import com.pathplanner.lib.util.PIDConstants;
19+
import com.pathplanner.lib.util.PathPlannerLogging;
1920
import com.pathplanner.lib.util.ReplanningConfig;
2021
import edu.wpi.first.math.controller.PIDController;
2122
import edu.wpi.first.math.geometry.Pose2d;
@@ -550,6 +551,31 @@ public void driveToPose(Pose2d targetPose) {
550551
pathfindCommand.schedule();
551552
}
552553

554+
public void driveToPath(String pathName) {
555+
this.setAlignState(AlignState.MANUAL);
556+
557+
PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);
558+
559+
// if(DriverStation.getAlliance() === Alliance.Blue) {
560+
// path.flipPath();
561+
// }
562+
563+
PathConstraints constraints =
564+
new PathConstraints(
565+
3.0,
566+
4.0,
567+
Constants.ConversionConstants.kDegreesToRadians * 540,
568+
Constants.ConversionConstants.kDegreesToRadians * 720);
569+
570+
PathPlannerLogging.setLogTargetPoseCallback(
571+
(target) -> {
572+
Logger.recordOutput("targetPose", target);
573+
});
574+
575+
pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0);
576+
pathfindCommand.schedule();
577+
}
578+
553579
public void stopDriveToPose() {
554580
if (pathfindCommand != null) {
555581
pathfindCommand.cancel();
@@ -562,50 +588,12 @@ public void setPoseTarget(Pose2d pose) {
562588
targetTightPose = pose;
563589
}
564590

565-
public Pose2d getEndgamePose() {
566-
// Blue Alliance Poses
567-
Pose2d leftClimbPose2d = new Pose2d(4.61, 4.48, Rotation2d.fromDegrees(-60));
568-
Pose2d rightClimbPose2d = new Pose2d(4.66, 3.67, Rotation2d.fromDegrees(60));
569-
Pose2d farClimbPose2d = new Pose2d(5.38, 4.11, Rotation2d.fromDegrees(180));
570-
571-
if (DriverStation.getAlliance().isPresent()
572-
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
573-
// Red Alliance Poses
574-
leftClimbPose2d = new Pose2d(11.882, 3.67, Rotation2d.fromDegrees(120));
575-
rightClimbPose2d = new Pose2d(11.932, 4.48, Rotation2d.fromDegrees(-120));
576-
farClimbPose2d = new Pose2d(11.162, 4.11, Rotation2d.fromDegrees(0));
577-
}
578-
579-
double distanceToTargetLeft =
580-
Math.hypot(
581-
getFieldToRobot.get().getX() - leftClimbPose2d.getX(),
582-
getFieldToRobot.get().getY() - leftClimbPose2d.getY());
583-
double distanceToTargetRight =
584-
Math.hypot(
585-
getFieldToRobot.get().getX() - rightClimbPose2d.getX(),
586-
getFieldToRobot.get().getY() - rightClimbPose2d.getY());
587-
double distanceToTargetFar =
588-
Math.hypot(
589-
getFieldToRobot.get().getX() - farClimbPose2d.getX(),
590-
getFieldToRobot.get().getY() - farClimbPose2d.getY());
591-
592-
Pose2d targetPose = new Pose2d(new Translation2d(), Rotation2d.fromDegrees(180));
593-
594-
if (distanceToTargetLeft < distanceToTargetRight
595-
&& distanceToTargetLeft < distanceToTargetFar) {
596-
targetPose = leftClimbPose2d;
597-
} else if (distanceToTargetRight < distanceToTargetLeft
598-
&& distanceToTargetRight < distanceToTargetFar) {
599-
targetPose = rightClimbPose2d;
600-
} else {
601-
targetPose = farClimbPose2d;
602-
}
603-
604-
return targetPose;
591+
public void driveToSpeaker() {
592+
driveToPath("speaker");
605593
}
606594

607-
public void driveToSpeaker() {
608-
driveToPose(new Pose2d(AllianceUtil.getFieldToSpeaker(), new Rotation2d()));
595+
public void driveToSource() {
596+
driveToPath("shopSource");
609597
}
610598

611599
public void driveToEndgame() {
@@ -635,29 +623,15 @@ public void driveToEndgame() {
635623
getFieldToRobot.get().getX() - farClimbPose2d.getX(),
636624
getFieldToRobot.get().getY() - farClimbPose2d.getY());
637625

638-
PathPlannerPath path = null;
639-
640626
if (distanceToTargetLeft < distanceToTargetRight
641627
&& distanceToTargetLeft < distanceToTargetFar) {
642-
path = PathPlannerPath.fromPathFile("LeftEndgame");
628+
driveToPath("leftEndgame");
643629
} else if (distanceToTargetRight < distanceToTargetLeft
644630
&& distanceToTargetRight < distanceToTargetFar) {
645-
path = PathPlannerPath.fromPathFile("RightEndgame");
631+
driveToPath("rightEndgame");
646632
} else {
647-
path = PathPlannerPath.fromPathFile("FarEndgame");
633+
driveToPath("farEndgame");
648634
}
649-
650-
this.setAlignState(AlignState.MANUAL);
651-
652-
PathConstraints constraints =
653-
new PathConstraints(
654-
3.0,
655-
4.0,
656-
Constants.ConversionConstants.kDegreesToRadians * 540,
657-
Constants.ConversionConstants.kDegreesToRadians * 720);
658-
659-
pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0);
660-
pathfindCommand.schedule();
661635
}
662636

663637
public boolean isAligned() {

0 commit comments

Comments
 (0)