Example Auto
This pages gives an overview of how to create an autonomous routine using PedroPathing. In this example, the robot scores its preloads and completes three cycles of picking up and scoring, and showcases several different methods for building paths. A path progression system is also implemented, allowing the routine to advance based on time, position, or other custom conditions.
Step One: Setting up imports and variables
Ensure to include the required imports for the autonomous. Note: Make sure the package statement aligns with that of where your class files are actually located. Android Studio should automatically generate it when creating a class
package org.firstinspires.ftc.teamcode.pedroPathing; // make sure this aligns with class location
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.Path;
import com.pedropathing.paths.PathChain;
import com.pedropathing.util.Timer;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@Autonomous(name = "Example Auto", group = "Examples")
public class ExampleAuto extends OpMode {
private Follower follower;
private Timer pathTimer, actionTimer, opmodeTimer;
private int pathState;Step Two: Initializing poses
Poses are points on the field coordinate system that define the (x, y, heading) of your robot during the
autonomous. Pedro’s coordinate system spans an interval of [0, 144] on both the x and y axes, with (0, 0) defined
as the bottom-left corner of the field. Even though Pedro uses a different coordinate system than RR, you can convert
any roadrunner pose by adding +72 both the x and y. Below are the poses for our example autonomous:
private final Pose startPose = new Pose(28.5, 128, Math.toRadians(180)); // Start Pose of our robot.
private final Pose scorePose = new Pose(60, 85, Math.toRadians(135)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle.
private final Pose pickup1Pose = new Pose(37, 121, Math.toRadians(0)); // Highest (First Set) of Artifacts from the Spike Mark.
private final Pose pickup2Pose = new Pose(43, 130, Math.toRadians(0)); // Middle (Second Set) of Artifacts from the Spike Mark.
private final Pose pickup3Pose = new Pose(49, 135, Math.toRadians(0)); // Lowest (Third Set) of Artifacts from the Spike Mark.Step Three: Path Initializing
Now we need to build the paths for the auto, while setting the desired path constraints. Below is our example on initializing paths. It is necessary to do this so that all the paths are built before the auto starts.
There are two major types of paths components: BezierCurves and BezierLines.
- BezierCurves are curved, and require >= 3 points. There are the start and end points, and the control points. Control points manipulate the curve between the start and end points.
- BezierLines are straight, and require 2 points. There are the start and end points.
There are also constraints which you can add to modify how your robot follows a path. To understand all of them, navigate to Constraints. To understand how heading interpolation works, navigate to Interpolation.
private Path scorePreload;
private PathChain grabPickup1, scorePickup1, grabPickup2, scorePickup2, grabPickup3, scorePickup3;
public void buildPaths() {
/* This is our scorePreload path. We are using a BezierLine, which is a straight line. */
scorePreload = new Path(new BezierLine(startPose, scorePose));
scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading());
/* Here is an example for Constant Interpolation
scorePreload.setConstantInterpolation(startPose.getHeading()); */
/* This is our grabPickup1 PathChain. We are using a single path with a BezierLine, which is a straight line. */
grabPickup1 = follower.pathBuilder()
.addPath(new BezierLine(scorePose, pickup1Pose))
.setLinearHeadingInterpolation(scorePose.getHeading(), pickup1Pose.getHeading())
.build();
/* This is our scorePickup1 PathChain. We are using a single path with a BezierLine, which is a straight line. */
scorePickup1 = follower.pathBuilder()
.addPath(new BezierLine(pickup1Pose, scorePose))
.setLinearHeadingInterpolation(pickup1Pose.getHeading(), scorePose.getHeading())
.build();
/* This is our grabPickup2 PathChain. We are using a single path with a BezierLine, which is a straight line. */
grabPickup2 = follower.pathBuilder()
.addPath(new BezierLine(scorePose, pickup2Pose))
.setLinearHeadingInterpolation(scorePose.getHeading(), pickup2Pose.getHeading())
.build();
/* This is our scorePickup2 PathChain. We are using a single path with a BezierLine, which is a straight line. */
scorePickup2 = follower.pathBuilder()
.addPath(new BezierLine(pickup2Pose, scorePose))
.setLinearHeadingInterpolation(pickup2Pose.getHeading(), scorePose.getHeading())
.build();
/* This is our grabPickup3 PathChain. We are using a single path with a BezierLine, which is a straight line. */
grabPickup3 = follower.pathBuilder()
.addPath(new BezierLine(scorePose, pickup3Pose))
.setLinearHeadingInterpolation(scorePose.getHeading(), pickup3Pose.getHeading())
.build();
/* This is our scorePickup3 PathChain. We are using a single path with a BezierLine, which is a straight line. */
scorePickup3 = follower.pathBuilder()
.addPath(new BezierLine(pickup3Pose, scorePose))
.setLinearHeadingInterpolation(pickup3Pose.getHeading(), scorePose.getHeading())
.build();
}Step Four: Managing Path States
The switch is called continuously and runs the pathing, at certain points, it triggers the action state. The pathState variable in the switch statement
will track the robot's movement throughout the autonomous. Every time the switch changes case, it will reset the timer. The followPath() function sets the follower to run
the specific path, but does NOT wait for it to finish before moving on. Rather, the robot will transition between path states based on a specified condition in the if statement
(known as a Finite State Machine, or FSM).
Below is an example state manager with explanations on what each case does, and how to modify it to fit your own routine.
public void autonomousPathUpdate() {
switch (pathState) {
case 0:
follower.followPath(scorePreload);
setPathState(1);
break;
case 1:
/* You could check for
- Follower State: "if(!follower.isBusy()) {}"
- Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}"
- Robot Position: "if(follower.getPose().getX() > 36) {}"
*/
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(grabPickup1,true);
setPathState(2);
}
break;
case 2:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup1Pose's position */
if(!follower.isBusy()) {
/* Grab Sample */
/* Since this is a pathChain, we can have Pedro hold the end point while we are scoring the sample */
follower.followPath(scorePickup1,true);
setPathState(3);
}
break;
case 3:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Sample */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(grabPickup2,true);
setPathState(4);
}
break;
case 4:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup2Pose's position */
if(!follower.isBusy()) {
/* Grab Sample */
/* Since this is a pathChain, we can have Pedro hold the end point while we are scoring the sample */
follower.followPath(scorePickup2,true);
setPathState(5);
}
break;
case 5:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Score Sample */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(grabPickup3,true);
setPathState(6);
}
break;
case 6:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup3Pose's position */
if(!follower.isBusy()) {
/* Grab Sample */
/* Since this is a pathChain, we can have Pedro hold the end point while we are scoring the sample */
follower.followPath(scorePickup3, true);
setPathState(7);
}
break;
case 7:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if(!follower.isBusy()) {
/* Set the state to a Case we won't use or define, so it just stops running an new paths */
setPathState(-1);
}
break;
}
}
/** These change the states of the paths and actions. It will also reset the timers of the individual switches **/
public void setPathState(int pState) {
pathState = pState;
pathTimer.resetTimer();
}Step Five: Init and Loop Methods:
The initialization phase sets up our paths and timers, while the loop ensures continuous updates during autonomous.
/** This is the main loop of the OpMode, it will run repeatedly after clicking "Play". **/
@Override
public void loop() {
// These loop the movements of the robot, these must be called continuously in order to work
follower.update();
autonomousPathUpdate();
// Feedback to Driver Hub for debugging
telemetry.addData("path state", pathState);
telemetry.addData("x", follower.getPose().getX());
telemetry.addData("y", follower.getPose().getY());
telemetry.addData("heading", follower.getPose().getHeading());
telemetry.update();
}
/** This method is called once at the init of the OpMode. **/
@Override
public void init() {
pathTimer = new Timer();
opmodeTimer = new Timer();
opmodeTimer.resetTimer();
follower = Constants.createFollower(hardwareMap);
buildPaths();
follower.setStartingPose(startPose);
}
/** This method is called continuously after Init while waiting for "play". **/
@Override
public void init_loop() {}
/** This method is called once at the start of the OpMode.
* It runs all the setup actions, including building paths and starting the path system **/
@Override
public void start() {
opmodeTimer.resetTimer();
setPathState(0);
}
/** We do not use this because everything should automatically disable **/
@Override
public void stop() {}Last updated on