Skip to content

Commit 3b77879

Browse files
CoolSpy3CoolSpy3
authored
Merge pull request #31 from DeepBlueRobotics/3015-swerve-pathing
Add Path Planner Support
2 parents a3866b2 + d4779e4 commit 3b77879

3 files changed

Lines changed: 140 additions & 0 deletions

File tree

‎src/main/java/org/carlmontrobotics/lib199/path/PPRobotPath.java‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
package org.carlmontrobotics.lib199.path;
2+
3+
import java.util.HashMap;
4+
import java.util.Map;
5+
6+
import com.pathplanner.lib.PathPlanner;
7+
import com.pathplanner.lib.PathPlannerTrajectory;
8+
9+
import edu.wpi.first.wpilibj2.command.Command;
10+
import edu.wpi.first.wpilibj2.command.InstantCommand;
11+
12+
//Findd Me
13+
public class PPRobotPath {
14+
15+
private PathPlannerTrajectory trajectory;
16+
private SwerveDriveInterface dt;
17+
private HashMap<String, Command> eventMap;
18+
19+
/**
20+
* Constructs a RobotPath Object
21+
*
22+
* @param pathName Name of the path
23+
* @param dt Drivetrain object
24+
* @param reversed Whether the path should be reversed
25+
* @param eventMap Map of event marker names to the commands that should run when reaching that marker.
26+
* This SHOULD NOT contain any commands requiring the same subsystems as this command, or it will be interrupted
27+
*/
28+
public PPRobotPath(String pathName, SwerveDriveInterface dt, boolean reversed, HashMap<String, Command> eventMap) {
29+
this.trajectory = PathPlanner.loadPath(pathName, dt.getMaxSpeedMps(), dt.getMaxAccelMps2(), reversed);
30+
this.dt = dt;
31+
this.eventMap = eventMap;
32+
}
33+
34+
/**
35+
* Gets a path command for the given path
36+
*
37+
* @param initializeDrivetrainPosition whether the robot should assume it is currently at the start of the path
38+
* @param stopAtEnd whether the robot should stop at the end
39+
* @return PathCommand
40+
*/
41+
public Command getPathCommand(boolean initializeDrivetrainPosition, boolean stopAtEnd) {
42+
Command command = dt.createPPAutoCommand(trajectory, eventMap);
43+
if(initializeDrivetrainPosition) {
44+
command = new InstantCommand(this::initializeDrivetrainPosition).andThen(command);
45+
}
46+
if (stopAtEnd) {
47+
command = command.andThen(new InstantCommand(dt::stop, dt));
48+
}
49+
return command;
50+
}
51+
52+
/**
53+
* Tells the drivetrain to assume that the robot is at the starting position of
54+
* this path.
55+
*/
56+
public void initializeDrivetrainPosition() {
57+
dt.setOdometry(trajectory.getInitialPose());
58+
}
59+
60+
/**
61+
* Adds an event to the event map
62+
* @param event The name of the event
63+
* @param command The command to run
64+
* This SHOULD NOT contain any commands requiring Drivetrain, or it will be interrupted
65+
*/
66+
public void addEvent(String event, Command command) {
67+
eventMap.put(event, command);
68+
}
69+
70+
/**
71+
* Adds multiple events to the event map
72+
* @param events The events to add
73+
*/
74+
public void addEvent(Map<String, Command> events) {
75+
eventMap.putAll(events);
76+
}
77+
}

‎src/main/java/org/carlmontrobotics/lib199/path/SwerveDriveInterface.java‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,12 @@
11
package org.carlmontrobotics.lib199.path;
22

3+
import java.util.Arrays;
4+
import java.util.HashMap;
35
import java.util.function.Supplier;
46

7+
import com.pathplanner.lib.PathPlannerTrajectory;
8+
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
9+
510
import edu.wpi.first.math.controller.PIDController;
611
import edu.wpi.first.math.controller.ProfiledPIDController;
712
import edu.wpi.first.math.geometry.Pose2d;
@@ -15,6 +20,7 @@
1520
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
1621
import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
1722
import edu.wpi.first.wpilibj2.command.Command;
23+
import edu.wpi.first.wpilibj2.command.Subsystem;
1824
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
1925

2026
public interface SwerveDriveInterface extends DrivetrainInterface {
@@ -63,6 +69,7 @@ public interface SwerveDriveInterface extends DrivetrainInterface {
6369

6470
/**
6571
* Configures the constants for generating a trajectory
72+
* WARNING: THIS METHOD IS NOT CALLED IF USING PPRobotPath!
6673
*
6774
* @param path The configuration for generating a trajectory
6875
*/
@@ -100,6 +107,27 @@ public default Command createAutoCommand(Trajectory trajectory, Supplier<Rotatio
100107
desiredHeading, this::drive, this);
101108
}
102109

110+
/**
111+
* Constructs a new PPSwerveControllerCommand that, when executed, will follow the
112+
* provided trajectory.
113+
*
114+
* @param trajectory The trajectory to follow.
115+
* @param eventMap Map of event marker names to the commands that should run when reaching that marker.
116+
* This SHOULD NOT contain any commands requiring Drivetrain, or it will be interrupted
117+
* @return PPSwerveControllerCommand
118+
*/
119+
public default Command createPPAutoCommand(PathPlannerTrajectory trajectory, HashMap<String, Command> eventMap) {
120+
Subsystem[] requirements = eventMap.values().stream().flatMap(command -> command.getRequirements().stream())
121+
.toArray(Subsystem[]::new);
122+
requirements = Arrays.copyOf(requirements, requirements.length + 1);
123+
requirements[requirements.length - 1] = this;
124+
requirements = Arrays.stream(requirements).distinct().toArray(Subsystem[]::new);
125+
PIDController[] pidControllers = Arrays.stream(getPIDConstants()).map(constants -> new PIDController(constants[0], constants[1], constants[2])).toArray(PIDController[]::new);
126+
pidControllers[2].enableContinuousInput(-Math.PI, Math.PI);
127+
// Call getOdometry in the supplier because the odometry object may be reset when the command is run
128+
return new PPSwerveControllerCommand(trajectory, () -> getOdometry().getPoseMeters(), getKinematics(), pidControllers[0], pidControllers[1], pidControllers[2], this::drive, requirements);
129+
}
130+
103131
/**
104132
* Sets odometry to the specified pose
105133
*

‎vendordeps/PathplannerLib.json‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
{
2+
"fileName": "PathplannerLib.json",
3+
"name": "PathplannerLib",
4+
"version": "2023.3.1",
5+
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
6+
"mavenUrls": [
7+
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
8+
],
9+
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
10+
"javaDependencies": [
11+
{
12+
"groupId": "com.pathplanner.lib",
13+
"artifactId": "PathplannerLib-java",
14+
"version": "2023.3.1"
15+
}
16+
],
17+
"jniDependencies": [],
18+
"cppDependencies": [
19+
{
20+
"groupId": "com.pathplanner.lib",
21+
"artifactId": "PathplannerLib-cpp",
22+
"version": "2023.3.1",
23+
"libName": "PathplannerLib",
24+
"headerClassifier": "headers",
25+
"sharedLibrary": false,
26+
"skipInvalidPlatforms": true,
27+
"binaryPlatforms": [
28+
"windowsx86-64",
29+
"linuxx86-64",
30+
"osxuniversal",
31+
"linuxathena"
32+
]
33+
}
34+
]
35+
}

0 commit comments

Comments
 (0)