BLine Paths

1https://raw.githubusercontent.com/edanliahovetsky/BLine-Lib/wpilib-2027/BLine-Lib-2027.json

1// BLine hands back speeds relative to the robot, so the request is robot-relative too.
2// Velocity asks each wheel for an exact speed, so the small corrections at the end of a path
3// still turn the wheels instead of stalling against friction.
4private final SwerveRequest.ApplyRobotVelocity pathRequest =
5    new SwerveRequest.ApplyRobotVelocity().withDriveRequestType(DriveRequestType.Velocity);
6 
7  // Teaches BLine how to drive this robot. After this, followPath can turn any path drawn in
8  // the BLine editor into a command.
9  pathBuilder =
10      new FollowPath.Builder(
11              DriveType.SWERVE,
12              this, // path commands require this mechanism
13              () -> getPose(), // where the robot is
14              pose -> resetPose(pose), // used when a path says where the robot starts
15              () -> getRobotVelocity(), // how fast it is moving, in its own directions
16              velocities -> driveRobotRelative(velocities), // drive like this
17              // Each controller pulls the robot back toward the path. TODO: tune on your robot.
18              new PIDController(2.0, 0.0, 0.0), // distance to the target: m/s per meter
19              // At 1.0 the turn lagged and stalled short of the end tolerance in simulation.
20              new PIDController(3.0, 0.0, 0.0), // heading error: rad/s per radian
21              new PIDController(0.2, 0.0, 0.0)) // drift off the line: m/s per meter
22          // Paths are drawn from the blue side. On red, BLine turns them around the field center.
23          .withDefaultShouldFlip();

1@Autonomous(name = "Auto")
2public class AutoOpMode extends PeriodicOpMode {
3  private final Selectable<Command> autoChooser = new Selectable<>();
4  private Command routine;
5 
6  public AutoOpMode(Robot robot) {
7    // Event triggers are the markers along a path. The key in quotes must match the marker's key
8    // in the BLine editor exactly. This one is a stand-in that only prints a line to the driver
9    // station console and the log. A robot with an intake would start the intake here instead.
10    FollowPath.registerEventTrigger(
11        "atMidpoint", () -> DataLogManager.log("Leave Start: reached the midpoint"));
12 
13    // Read the path file now, so a misspelled name fails when this OpMode is picked.
14    // withPoseReset puts odometry at the path's first point before it drives.
15    final var leaveStartPath = robot.drivetrain.followPath("leave-start").withPoseReset();
16 
17    // The routine owns no mechanism itself. The path command it awaits claims the drivetrain for
18    // as long as it drives, and a second path would go on the next line.
19    final Command leaveStart =
20        Command.noRequirements(
21                coroutine -> {
22                  coroutine.await(leaveStartPath);
23                })
24            .named("Leave Start");
25    final Command none = Command.noRequirements(coroutine -> {}).named("None");
26 
27    autoChooser.addDefault("Leave Start", leaveStart);
28    autoChooser.add("None", none);
29    Tunables.publish("Auto", autoChooser);
30  }
31 
32  @Override
33  public void start() {
34    routine = autoChooser.getSelected();
35    Scheduler.getDefault().schedule(routine);
36  }
37 
38  @Override
39  public void end() {
40    Scheduler.getDefault().cancel(routine);
41    FollowPath.clearPendingEventTriggers();
42  }
43 
44  @Override
45  public void close() {
46    Tunables.remove("Auto");
47  }
48}