Path Following in TeleOp

Path following isn’t limited only to the autonomous period—Marrow makes it possible to generate Bézier paths on the fly, perfect for TeleOp, where the robot could be anywhere on the field.

The AutoPath system takes the robot’s current pose, a target pose, and any obstacles in between. From this, it calculates an optimal path to the target while avoiding collisions. AutoPath can be used with any path-follower that supports Bézier curves.

Accurate localization is a must for this to work reliably. This often means re-localizing mid-teleop using AprilTags or by driving the robot to a known reference point on the field.

Use Cases

• Cycle paths: Automate and optimize cycles.

• Precision alignment: Help the driver line up to a scoring target.

• Parking assist: Let the robot drive itself to park.

Example Usage

In this setup:

• Pressing A starts the robot on a scoring path.

• Moving the joysticks immediately cancels the path and returns control to the driver.

This is by no means a functional TeleOp program.

public class TeleOpMode extends CommandOpMode {
    private DriveSubsystem drive = new DriveSubsystem();
    private Follower follower = new Follower(hardwareMap);
    private GamepadEx driverGamepad = new GamepadEx(gamepad1);

    @Override
    public void initialize() {
        Command goToScore = new FollowPathCommand(follower, XXXXXXXXXXXX);
    
        // Bind a button to trigger path following
        driverGamepad.getGamepadButton(GamepadKeys.Button.A)
                     .whenPressed(goToScore);

        // Cancel path if driver moves the joystick
        schedule(new RunCommand(() -> {
            if (driverGamepad.getLeftX() != 0 || driverGamepad.getLeftY() != 0 || driverGamepad.getRightX() != 0 || driverGamepad.getRightY() != 0) {
                CommandScheduler.getInstance().cancel(goToScore);
            }
        }));
        
        // Default command to use joysticks to drive
        drive.setDefaultCommand(new DriveCommand(gamepad1));
    }
    
    @Override
    public void run() {
        super.run();
        follower.update();
    }
}