Teleop navigation

Teleop navigation is for driver-requested movement to named field goals. It uses PathPlanner runtime pathfinding, then hands the final approach to DriveToPose.

This is intentionally separate from autonomous. BLine owns autonomous paths. PathPlanner owns runtime teleop pathfinding.

Named goals

Field goals live in FieldTargets. They are neutral sample goals in the template. Replace them with yearly scoring, intake, climb, or staging poses.

Goals should be authored from the blue origin. The helper transforms them for red alliance.

Good goal names are stable and meaningful:

• speaker-left
• amp-score
• source-intake
• processor-align

Avoid names like pose1 or test2 once the season starts. Bad names survive longer than anyone expects.

Driver controls

The template provides sample bindings:

• normal field-relative driving on sticks
• slow mode on left trigger
• heading reset
• X-lock
• sample navigation goals
• cancel/return-to-manual

Keep the cancel binding obvious. Drivers must be able to abandon a navigation command immediately.

PathPlanner navgrid

The navgrid lives in:

src/main/deploy/pathplanner/navgrid.json

Use PathPlanner to mark static obstacles. Commit the resulting file. The robot should not depend on an uncommitted navgrid from a developer laptop.

Regenerate the navgrid when field obstacles or robot assumptions change.

Navigation command flow

The normal flow is:

1. Driver presses a goal binding.
2. Code resolves the named goal for the current alliance.
3. PathPlanner pathfinds toward the goal.
4. DriveToPose performs terminal alignment.
5. Driver cancels or command finishes.

Vision loss should not silently change this behavior. It should log diagnostics and raise alerts.

Tuning

Tune after the drivetrain is correct.

Suggested order:

1. Validate field-relative drive.
2. Validate odometry over a known distance.
3. Tune BLine/autonomous path tracking if needed.
4. Tune DriveToPose.
5. Tune PathPlanner pathfinding constraints.
6. Test driver cancellation repeatedly.

If terminal alignment is poor, check the robot pose estimate before changing gains.

Safety expectations

Teleop navigation is a driver assist, not a driver replacement. Design it so a human can take back control quickly.

Expected behavior:

• manual driving is the default command
• navigation commands require the drivetrain subsystem
• cancel returns to manual drive
• slow mode remains available for manual control
• navigation state and selected goal are logged

Common mistakes

• Using BLine for teleop navigation.
• Adding PathPlanner autos because PathPlanner is already installed.
• Forgetting to alliance-transform named goals.
• Hiding navigation state from logs.
• Letting a navigation command fight the driver's manual command.
• Treating a sample goal as season-ready field configuration.