Pathfinding

The PathPlanner app wrote deploy/pathplanner/navgrid.json the first time it opened the project. It splits the field into 0.3 m squares and marks each one blocked or open. Open means the center of the robot can pass through without the bumpers touching anything.

That is why the default grid blocks a wide band around the hub, not just the hub. The margin is the bumper. Open the Navigation Grid page in the app to see it. Leave it as shipped until a run shows the bumper reaching an obstacle.

The search, AD*, runs on its own thread. Start it in the DriveMechanism constructor, before AutoBuilder.configure, so the first button press does not pay to load the grid.

1// Load deploy/pathplanner/navgrid.json and start the route search on its own thread, now,
2// so the first request does not pay for it.
3Pathfinding.ensureInitialized();

Then one method asks AutoBuilder for a pathfinding command. It uses the controller, config and alliance flip that configure already gave it.

1public Command pathfindTo(Pose2d goal) {
2  // Speed limits for routes the pathfinder makes up: 2 m/s, 2 m/s², and a turn rate of 3/4 of a
3  // turn per second. A drawn path carries its own limits; a found one gets these.
4  return AutoBuilder.pathfindToPose(
5      goal, new PathConstraints(2.0, 2.0, Math.toRadians(270), Math.toRadians(360), 12.0));
6}

Hold A in teleop and the robot drives itself to the middle of the neutral zone, around the hub.

1// Hold A: find a way around the hub to the middle of the neutral zone, and drive it. Let go
2// and the sticks take over again. The goal is blue-origin, like every pose on this robot.
3driver.a().whileTrue(drivetrain.pathfindTo(new Pose2d(7.5, 4.0, Rotation2d.ZERO)));

Let go and the command is canceled. A canceled pathfind sends no zero. What stops the robot is the joystick default command, which takes the drivetrain back the same loop and sends whatever the sticks say. Bind this anywhere without a drive default and the robot keeps its last request.

Pathfinding is good at crossing the field and poor at the last few centimeters. Send it to a pose near the target, then finish with Drive to Point.

Keeps rolling

No route

A goal on a blocked square is moved to the nearest open one, so that is not it. The open area around the goal is walled off from the robot. The command sends nothing and holds the drivetrain, so the last request stays on. Let go of A.

Clips the hub

Grid too thin

The center stayed on open squares and the bumper still hit. The robot is wider than the grid's margin. Paint more squares blocked around that obstacle.

Odd start

A bad pose

The route begins somewhere the robot is not. The search trusts getPose(), so fix odometry or vision before the grid.

1. In simulation, run the Leave Start auto so the robot ends on the near side of the hub.
2. Switch to Teleop, enable, and hold A. Watch Drivetrain/Pose on the 2D field in AdvantageScope.
3. Drive back with the sticks, hold A again, and let go halfway.