PedroPathing

If you're using Pedro Pathing, an Advanced Reactive Vector Follower developed by FTC Team 10158, NextFTC has a simple integration of this path following system in your autonomous programs. It's recommended that you first read and implement the Autonomous page into your code, before referring to this page.

This guide will show you how to use the basic following features of Pedro Pathing, using its follower, with the core features of NextFTC such as command groups and delays.

Let's begin!

Step 1: Create your OpMode

Just like with our first autonomous, let's create an OpMode and add our subsystems and bulk reading. This time, we'll also add a PedroComponent with our constants classes.

@Autonomous(name = "NextFTC PedroPathing Autonomous Program Kotlin")
class PedroPathingAutonomousProgram : NextFTCOpMode() {
   init {
      addComponents(
         SubsystemComponent(Lift, Claw),
         BulkReadComponent(),
         PedroComponent(FConstants::class.java, LConstants::class.java)
      )
   }
}

Step 2: Defining your PedroPathing Paths

We'll assume that you're following an autonomous similar to the PedroPathing Example Auto, so you should have two poses and one PathChain that's already been built in a buildPaths() function. Place your poses right below where you instantiate your subsystems, then place your paths below that. It should look like this:

private val startPose = Pose(9.0, 60.0, Math.toRadians(0.0))
private val finishPose = Pose(37.0, 50.0, Math.toRadians(180.0))

private val move = follower.pathBuilder()
    .addPath(BezierLine(Point(startPose), Point(finishPose)))
    .setLinearHeadingInterpolation(startPose.heading, finishPose.heading)
    .build()

Step 3: Constructing your Path Following Routine

Now, to actually run your paths as part of your routine, it's very straightforward! We're going to take our previous routine and create a new one, this time with path following. The routine will:

1. Simultaneously move your robot forward and raise the lift to the high position
2. Simultaneously open the claw and move the lift to the middle position
3. Wait for a second
4. Move the lift to the low position

We're going to start by creating a new routine:

val autonomousRoutine: Command

We will then create our main SequentialGroup. Since the first step of our routine is to simultaneously move the robot and move the lift to the high position, we'll also create a ParallelGroup that does this.

In order to actually have our robot follow a path, we can use the FollowPath command to tell our follower to follow a specific path or path chain.

In this case, we will use our path chain we defined before, called move.

val autonomousRoutine: Command
    get() = SequentialGroup(
        ParallelGroup(
            FollowPath(move),
            Lift.toHigh
        )
    )

For the second step, to simultaneously open the claw and move the lift to the middle position, we will simply create another ParallelGroup that does this

val autonomousRoutine: Command
    get() = SequentialGroup(
        ParallelGroup(
            FollowPath(move),
            Lift.toHigh
        ),
        ParallelGroup(
            Claw.open,
            Lift.toMiddle
        )
    )

And finally, our last two steps of our routine are adding a delay for a second, and then moving our lift to the low position.

val autonomousRoutine: Command
    get() = SequentialGroup(
        ParallelGroup(
            FollowPath(move),
            Lift.toHigh
        ),
        ParallelGroup(
            Claw.open,
            Lift.toMiddle
        ),
        Delay(1.0),
        Lift.toLow
    )

Step 4: Running your PedroPathing routine

Finally, we can run our routine by overriding the onStartButtonPressed() function.

override fun onStartButtonPressed() {
    autonomousRoutine()
}

Final Result

That's everything! This completes the integration of Pedro Pathing with NextFTC in your autonomous program. Hopefully, you can also use what you've learned from this guide to create more complex routines, combining path following with other robot actions.

Here is the final result:

@Autonomous(name = "NextFTC PedroPathing Autonomous Program Kotlin")
class PedroPathingAutonomousProgram : NextFTCOpMode() {
    init {
        addComponents(
            SubsystemComponent(Lift, Claw),
            BulkReadComponent(),
            PedroComponent(FConstants::class.java, LConstants::class.java)
        )
    }

    private val startPose = Pose(9.0, 60.0, Math.toRadians(0.0))
    private val finishPose = Pose(37.0, 50.0, Math.toRadians(180.0))

    private val move = follower.pathBuilder()
        .addPath(BezierLine(Point(startPose), Point(finishPose)))
        .setLinearHeadingInterpolation(startPose.heading, finishPose.heading)
        .build()

    private val autonomousRoutine: Command
        get() = SequentialGroup(
            ParallelGroup(
                FollowPath(move),
                Lift.toHigh
            ),
            ParallelGroup(
                Claw.open,
                Lift.toMiddle
            ),
            Delay(1.0.sec),
            Lift.toLow
        )

    override fun onStartButtonPressed() {
        autonomousRoutine()
    }
}