Alright. Here's a pretty quick and pretty cool tutorial on using Pathfinder. Note that this is only a really basic implementation of Pathfinder, and there's way cooler things you can do.
Here's a couple of important notices. Or one. Or more, I'm not entirely sure to be honest. This is NOT a tutorial - these are just important notices that you should PROBABLY read before using the library.
- Pathfinder2 is designed to operate with exclusively positive points. You CAN use negative points, but the library was not designed with that in mind and may not function as intended.
- Tolerance values cannot and should not be negative.
Okay. Here's a really quick overview of a couple of the most important concepts. Note that this does not fully encompass everything you can do.
- Your robot has a drivetrain. This is done with the Drive interface. There are
several ways you can implement a Drive, and there are a couple of cool
implementations, such as
MeccanumDrive. - Your robot has an odometry system. This thing just has to tell us where the robot is. It doesn't really matter how you implement odometry - I've used three-wheel odometry before, and it's worked perfectly.
Code time!
public class PathfinderGuide {
// You shouldn't actually use "new Motor()" for each of these - you
// should make/use your own motors.
private final Drive drive = new MeccanumDrive(
new Motor(),
new Motor(),
new Motor(),
new Motor()
);
// Once again, you shouldn't use "simulated odometry" - you should
// implement your own, maybe check out three wheel odometry?
private final Odometry odometry = new SimulatedOdometry();
}Cool. We've got those working now, right?
Next up, we have to construct a robot.
public class PathfinderGuide {
// ...
private final Robot robot = new Robot(drive, odometry);
}Alright, there's our robot. Only a couple more things we need to do before we can get Pathfinder actually working.
public class PathfinderGuide {
// you'll have to figure out a turn controller that works for you
private final Controller turnController = new GenericTurnController(0.5);
// you can read some documentation on this if you're curious
private final FollowerGenerator followerGenerator = new GenericFollowerGenerator(turnController);
}Okay, Pathfinder time!
public class PathfinderGuide {
// ...
private final Pathfinder pathfinder = new Pathfinder(robot, followerGenerator);
}There we go! In total, we have...
public class PathfinderGuide {
private final Drive drive = new MeccanumDrive(
new Motor(),
new Motor(),
new Motor(),
new Motor()
);
// Once again, you shouldn't use "simulated odometry" - you should
// implement your own, maybe check out three wheel odometry?
private final Odometry odometry = new SimulatedOdometry();
private final Robot robot = new Robot(drive, odometry);
private final Controller turnController = new GenericTurnController(0.05);
private final FollowerGenerator followerGenerator = new GenericFollowerGenerator(turnController);
private final Pathfinder pathfinder = new Pathfinder(robot, followerGenerator);
}There's a ton of functionality in this library, but for the sake of keeping things simple, we'll only go over the most basic concepts.
The most important thing you can do is tick Pathfinder - if you don't tick it, nothing happens. For example:
public class PathfinderGuide {
private Pathfinder pathfinder;
public void loop() {
pathfinder.tick();
}
}The concept here is that Pathfinder is running in a loop - it constantly checks its state (where it is, what the chassis is doing, where it's supposed to be going) and then decides what to do from there.
When you instruct Pathfinder to do something, you need to wait for it to finish doing what you asked it to do or cancel it.
public class PathfinderGuide {
private Pathfinder pathfinder;
public void moveAround() {
pathfinder.goTo(new PointXYZ(10, 10, 45));
while (pathfinder.isActive()) {
// This line isn't necessary, but it's a good programming practice
// to use something like this in your loops.
Thread.onSpinWait();
pathfinder.tick();
}
}
}But what if we want to cancel it? Let's say we want to check for a gamepad button being pressed - what do we do then?
public class PathfinderGuide {
private Pathfinder pathfinder;
public void maybeMoveAround() {
pathfinder.goTo(new PointXYZ(10, 10, 45));
while (pathfinder.isActive()) {
pathfinder.tick();
// If the A button on the gamepad is being held down, cancel
// following the path.
if (gamepad.a) {
pathfinder.cancel();
}
}
}
}Now let's follow some more paths, shall we? We're going to learn about more concepts - exciting! Right! Yeah...
Trajectories and followers are at the heart of Pathfinder's operation. I'd encourage you to go read the Javadocs for those (maybe just check out these pages)
Trajectories are paths your robot can follow. Followers are the things that make your robot follow the paths. Let's take a look at a simple example of a trajectory - a linear trajectory.
public class PathfinderGuide {
public void learningAboutTrajectories() {
// Create a new angle at (10, 10), with the robot facing
// 45 degrees.
PointXYZ target = new PointXYZ(10, 10, Angle.deg(45));
// Speed can be any value between 0 and 1 - 0 isn't moving, 1 is
// as fast as possible.
double speed = 0.5;
// The "tolerance." Basically, Pathfinder has a tolerance for when
// you're at a certain position. If you're at (9.5, 9.5) and your
// target is (10, 10), you're technically 0.707 units away from the
// target (we use the distance formula here). Because 0.707 is less
// than 1.0, Pathfinder goes "hey, we've reached our target." If
// you're at (9.0, 9.0), your distance is 1.414 units away from your
// target (sqrt2). 1.414 is greater than 1.0, so Pathfinder knows it
// hasn't reached it target yet.
double tolerance = 1.0;
// The angle tolerance - basically the same thing with the regular
// tolerance, but for the angle the robot is/should be facing.
Angle angleTolerance = Angle.deg(15);
// Let's create a linear trajectory. It requires four parameters:
// 1. a target/destination point
// 2. a speed to move at (0.0 - 1.0)
// 3. a tolerance value
// 4. an angle tolerance value
Trajectory trajectory = new LinearTrajectory(
target,
speed,
tolerance,
angleTolerance
);
// Tell Pathfinder to follow the trajectory.
pathfinder.followTrajectory(trajectory);
while (pathfinder.isActive()) {
Thread.onSpinWait();
pathfinder.tick();
}
}
}The name of the library is quite literally Pathfinder - it wouldn't be
unreasonable to assume that you'd be able to generate paths. How exactly would
one go about doing that, you may ask? Well, it's not all that challenging.
Before we can get into actually generating paths, we need to go over the
concept of a Zone. See me.wobblyyyy.pathfinder2.geometry.Shape and
me.wobblyyyy.pathfinder2.zones.Zone. In short, a zone is an area of the
field, defined by a shape (say, a rectangle or a circle) that Pathfinder
will not allow the robot to enter when generating a path. If you have an
obstacle on the field, you'd want to define that obstacle as a zone - this
means you'll be able to avoid the obstacle, which is probably your goal.