introduction

Welcome to the Cookbook. Enjoy your stay :)

The Cookbook is essentially a "handbook" maintained by the community for common (or not) issues that people have faced. The original motivation for this was to better prepare my team (4017) for when I left, since we've gained so much knowledge over just a single year that may eventually be lost. Throughout our region, I have seen many teams be completely lost after a great programmer or designer leaves, because they were simply not taught.

While a list of problems and solutions is not "teaching one how to fish", it serves as an incredibly helpful guide to just get things working. I hope by making my lil' cookbook open source, everyone can both use and build on my limited knowledge base.

If you (or anyone) has a problem, I'd love if you could make a lil' "recipe" and help anyone else in the future :)

Tags

Cookbook includes a set of quick aliases to many of the articles. These can be easily accessed using tools like Carl custom tags. One is available here: https://carl.gg/t/2167631

Here is a list of all of them:

!cb tags -> this page

!cb help -> this page

!cb as -> https://cookbook.dairy.foundation/intro_to_programming/setup.html

!cb git -> https://cookbook.dairy.foundation/intro_to_programming/intro_to_git.html

!cb jdk -> https://cookbook.dairy.foundation/gradle/downgrading_gradle_jdk/downgrading_gradle_jdk.html

!cb gradle -> https://cookbook.dairy.foundation/gradle/dont_upgrade/dont_upgrade.html

!cb emptylist -> https://cookbook.dairy.foundation/roadrunner_10/null_list_error_in_rr_10.html

!cb builder -> https://cookbook.dairy.foundation/roadrunner_10/complete_trajectorybuilder_reference.html

!cb bump -> https://cookbook.dairy.foundation/roadrunner_056/is_the_bump_on_manual_feedforward_tuner_normal.html

!cb ffbump -> https://cookbook.dairy.foundation/roadrunner_056/is_the_bump_on_manual_feedforward_tuner_normal.html

!cb overshoot -> https://cookbook.dairy.foundation/roadrunner_056/is_the_bump_on_manual_feedforward_tuner_normal.html

!cb null -> https://cookbook.dairy.foundation/common_ds_errors/npe_at_init/npe_at_init.html

!cb npe -> https://cookbook.dairy.foundation/common_ds_errors/npe_at_init/npe_at_init.html

!cb pid -> https://cookbook.dairy.foundation/pidf_controllers/integrating_a_custom_PIDF_controller.html

!cb pidsync -> https://cookbook.dairy.foundation/pidf_controllers/syncing_two_linear_slide_motors_using_a_pidf_controller/syncing_two_linear_slide_motors_using_a_pidf_controller.html

!cb usb -> https://cookbook.dairy.foundation/electrical/why_we_should_only_use_usb_30.html

!cb usb3 -> https://cookbook.dairy.foundation/electrical/why_we_should_only_use_usb_30.html

!cb odo -> https://cookbook.dairy.foundation/electrical/how_to_wire_odometry_pods.html

!cb kotlin -> https://cookbook.dairy.foundation/misc/why_kotlin/why_kotlin.html

!cb loop -> https://cookbook.dairy.foundation/improving_loop_times/improving_loop_times.html

!cb looptimes -> https://cookbook.dairy.foundation/improving_loop_times/improving_loop_times.html

!cb pedrovsrr -> https://cookbook.dairy.foundation/misc/pedro_vs_roadrunner.html

!cb rrvspedro -> https://cookbook.dairy.foundation/misc/pedro_vs_roadrunner.html

!cb tab -> https://cookbook.dairy.foundation/roadrunner_10/complete_trajectorybuilder_reference.html

!cb trajectory -> https://cookbook.dairy.foundation/roadrunner_10/complete_trajectorybuilder_reference.html

Development Environment Setup

This is a guide designed to assist new FTC programmers setup the Android Studio environment to program a robot.

Ingredients

A Computer that has the required specs for Android Studio and installing Java.
Access to Admin Permissions on aforementioned computer.

Software to be Installed

FTC SDK: FtcRobotController
IDE: Android Studio
Java: Any recent version of Java
ADB: Android Debug Bridge

Recipe (Installation)

1. Download and Install Java:
   NOTE: Installing Java may require Admin Permissions.
   • Download the latest version of Java.
   • Run the installer and follow the on-screen instructions.
     
     
2. Download and Install Android Studio:
   NOTE: Installing Android Studio may require Admin Permissions.
   • Download the latest version of Android Studio.
   • Run the installer and follow the on-screen instructions.
   • Open Android Studio.
     
     
3. Download and Open FtcRobotController:
   • In the FtcRobotController GitHub repository, press the blue code button and press download zip.
     • You can alternatively use Github Desktop to open FtcRobotController in Android Studio, which is not covered in this Recipe.
   • Extract the contents of the zip file to a folder (typically in your Downloads or Documents folder).
   • In Android Studio, press File → Open (⌘ + O on Mac or Win + O on Windows).
   • Select the folder you extracted the zip file to, and press open. DO NOT open any folder inside the extracted folder.
     • On MacOS, you can alternatively drag the folder from Finder onto the Android Studio icon in your taskbar to open the folder.
   • Wait for the project to load. You should end up with 3 folders in the Android view panel (which you should automatically be moved to once ready): FtcRobotController, TeamCode, and Gradle Scripts.
     
     
4. Install ADB:
   NOTE: Installing ADB may require Admin Permissions.
   • Download the latest version of ADB. Press on one of the three links depending on your operating system.
   • Extract the contents of the zip file to a folder (typically in your Downloads or Documents folder).
   • Add the folder to your system's PATH variable:
     • Windows:
       • Open the start menu and search for "Environment Variables", and press enter.
       • Click on "Edit the system environment variables".
       • Click on "Environment Variables".
       • In the "System variables" section, find the "Path" variable and click "Edit".
       • Click "New" and paste the path to the folder where you extracted the zip file.
       • Click "OK" on all the windows.
     • Mac:
       

       • Option 1 - Using Homebrew (Highly Recommended)
         Homebrew is a package manager for Mac. This is the easiest way and will provide automatic updates.

         • Install the Homebrew package manager by running the following command in a terminal:

         /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install.sh)"
         

         • Then, run this command to install ADB using Homebrew:

         brew install android-platform-tools
         

       • Option 2 - Manual Installation

         • Go to your Downloads folder with this command in terminal:

         cd ~/Downloads/
         

         • Then, to avoid deleting the ADB files, move the downloaded file to a new folder (the commands below should still work with modern versions of MacOS):

         mkdir ~/.android-sdk-macosx
         mv platform-tools/ ~/.android-sdk-macosx/platform-tools
         

         • Add platform-tools to your path

         echo 'export PATH=$PATH:~/.android-sdk-macosx/platform-tools/' >> ~/.bash_profile
         

         • Reload your terminal profile (or restart your terminal):

         source ~/.bash_profile
         

     • Linux:
       ADB should already be installed by default with the installation of Android Studio. If not, you can use the following steps below to install ADB manually:
       
       
       • Open a terminal window and run the following command:

         nano ~/.bashrc
         

         • Add the following line to the file:

         export PATH=$PATH:/path/to/adb/folder
         

         • Press Ctrl + X, then Y, then Enter to save the file.
         • Run the following command:

         source ~/.bashrc
         

   • Finally, to check that your properly installed ADB, in the Android Studio Terminal (Control + F12 on Windows or ⌘ + F12 on Mac), or on your default/preferred terminal, run the following command:

   adb devices
   

   • If you see a message saying List of devices attached, you have successfully installed ADB (even if there are no devices attached).

Congratulations! You have successfully installed the necessary software to program an FTC robot. You can now start programming your robot.

Troubleshooting

• If you have questions/issues with the installation process, the Unofficial FTC Discord has many experienced programmers who can help you with all sorts of issues, including installation issues.
• Last Updated: 2024-05-30

Introduction to Git and GitHub

This guide is designed to help FTC teams understand how to use Git and GitHub to track changes, collaborate better, and recover previous versions. There are often multiple ways to achieve the same task when using Git, so this guide will explain the method we think is the easiest.

This guide also assumes you are using the base FIRST Tech Challenge SDK. If you are instead using the RoadRunner quickstart or similar, use that in place of the SDK.

Git vs GitHub

Git is a version control system that tracks changes in code over time, allowing you to collaborate effectively. GitHub is a platform that hosts Git repositories. Though there are many Git hosting platforms, including GitLab and Bitbucket, this guide focuses on GitHub because it is the easiest to use with the FTC SDK.

Ingredients

1. Internet access
2. A computer
3. Android Studio
4. A GitHub account and organization.
   • To create an account, follow the steps on GitHub.
   • While creating an organization is optional, it is highly recommended for FTC teams. Follow the directions on GitHub.
   • Note that user accounts can only create one fork of a repository, while organization accounts can create multiple. This makes it easier for your team to set up a repository for each season.

Recipe

0. Installing Git

The easiest way to install Git on your device is to download it from Git's download page. Select your operating system and follow the instructions on the website.

1. Forking the Repository

This step only needs to be done once each season.

A Fork on GitHub is a copy of another repository on GitHub from one account to another account. The new forked repository retains a parent-child relationship with the origin repository. Forks are typically used when software will have an independent line of development, such as when FTC teams develop their own team code using the FIRST-Tech-Challenge/FtcRobotController repository as a basis. FTC teams should create a Fork of the FIRST-Tech-Challenge/FtcRobotController repository as a convenient way to manage their software development process. Thanks to the parent-child relationship, when changes are made to the parent repository those changes can be easily tracked and fetched/merged into the forked repository, keeping the forked repository up to date.

• The FIRST Tech Challenge documentation

First, open the FtcRobotController repository. The FtcRobotController repo is the Software Development Kit (SDK) provided by FIRST that allows you to write your own robot code.

Once you have opened the repo, click the Fork button in the upper-right-hand corner. That will bring you to a page that looks like this:

Under the Owner dropdown, select your organization (if you elected to create one), as opposed to your individual user account. Under Repository name, I recommend naming your repo after the current FTC season name (such as Into The Deep or CenterStage), or by the year (such as 2024). Finally, press Create fork to create your own copy of the SDK repository.

1.5 Logging into GitHub on Android Studio

First, open your GitHub token settings, either by clicking on that link or by going to Account Settings → Developer Settings → Tokens (Classic). Press Generate new token (classic) at the top and that will take you to a page that looks like this:

For Note, write the use case of the token, such as "Android Studio." For Expiration, select No expiration, which may cause GitHub to warn you. For Select scopes, select repo, workflow, read:org, and gist. Finally, click Generate token and copy it.

Now open Android Studio. Open your settings (under File then Settings) and then go to Version Control -> GitHub. In the top left corner of the box, press the + icon and Log in with token..., and paste in the token you just generated.

2. Opening Your Fork in Android Studio

This step needs to be done by everyone who intends on programming for your team.

First, at the top right of your new repository, press the green Code button. Under that tab, copy the HTTP url of your repo.

Next, open Android Studio and navigate to the New Project from Version Control menu. To do that, do File -> New -> Project from Version Control, which should bring you to a menu that looks like this:

For the URL, paste in the link that you just copied from your repo, then press Clone. Android Studio will then download your project and build it through Gradle, which may take a few minutes; you can monitor this process using the progress bar in the bottom right.

Once this is complete, your project is ready to use, and you can start coding as normal.

3. Your First Commit

Now that you've made some changes, you should create a commit to snapshot your changes and push (upload) them to GitHub. To do this, press the button on the left side that looks like a line with a circle on it (just like the circles in the above image) to open the Commit menu in Android Studio. That will look like this:

The Changes section will show the files you have edited. Select the files you want to commit by clicking on the checkbox next to them, or use the checkbox in the top left to select all of them.

Finally, write a commit message in the box in the lower portion of the menu to describe what you've changed. In this example, I added a MecanumChassis wrapper and edited some other files, so that's what I wrote in my commit message.

Once you're done, press Commit and Push... which will commit your changes and push them to GitHub's copy of your repository.

In some situations (such as when you are offline, or when a push fails), you may also prefer to just hit the Commit button to save an offline snapshot of your changes, and then later click your branch title in the top right, which displays the following options:

Click Push..., and then Push in the bottom right of the menu that comes up after that.

4. Pulling from GitHub

Once one person has committed a change, the other programmers on your team will want to download or pull those changes from GitHub. To do this, click on your branch and then the Update Project icon or button in the top left, as shown below:

This will ask you whether you want to Merge or to Rebase the incoming changes. Merging is simpler, so we will explain it here; select it and hit OK.

Most of the time that will be all that is necessary to download all the incoming changes, and you will immediately be able to resume coding. However, occasionally when multiple people edit the same file at the same time, a Merge Conflict can occur. This can appear as a Conflict pop up as shown below.

See the official JetBrains documentation for what to do in this scenario. Make sure to commit after the merge is complete.

Additional Features

Updating from the FIRST SDK

Throughout the season, the FIRST SDK sometimes updates to new versions. To incorporate these changes into your codebase, we need to add the SDK as a remote repository and then pull in its changes.

First, open a terminal using the button in the bottom left.

Next, the first time you update, run this command in the terminal: git remote add sdk https://github.com/FIRST-Tech-Challenge/FtcRobotController/ This will add the FIRST SDK as a remote repository named sdk. You will only have to do this once.

Next, each time you want to update, run this command: git pull sdk master --no-rebase This will pull the changes from the master branch of the sdk remote repository. We use --no-rebase here to ensure that we merge instead of rebasing.

Finally, make a new commit to incorporate the changes into your repository. There will very likely be merge conflicts, review step 4 to learn how to deal with those.

Creating Branches

A branch allows you to separate your codebase into multiple versions, which can be developed individually and combined later. Each branch can have its own set of commits. In the following image, each circle represents a commit in the branch.

Some teams prefer to create a new branch for each feature that they create. To do that in AS, in the top menu to go Git -> New Branch, and type in the name of that feature. AS will automatically checkout that branch, meaning all future commits from your client will be to that branch.

Merging Branches

To merge changes from one branch onto another through Android Studio, open the Git menu as shown below.

Now, right-click on the branch you intend on merging from and press Merge origin/<branch> into <branch>. This will simply update those files in your local copy of the code with the changes from the other branch. Note that this can also lead to Merge Conflicts as explained in step 4. After the merge is complete, make sure to commit the new combined code.

It is also possible to merge changes online through GitHub using Pull Requests. Pull Requests also allow others to easily review your changes.

At this point, we're going back to the GitHub website. Open your repository and hit the Pull Requests tab in the top left, which will open a page that looks somewhat like this:

Make sure that both repositories are the same (your repo). Then, for base, select master, and for compare select whatever branch you were working with. Press Create pull request and type the name and description of the commit(s) you are working with, and then press Create pull request again.

At this point, GitHub will automatically determine if there are merge conflicts. See GitHub's official documentation for information on how to resolve them, if they occur. Once any conflicts are resolved, and you are ready to merge the branches (potentially after getting approval from your team), select the Merge pull request button to accept the pull request.

Last updated: 2024-10-06

Downgrading the Gradle JDK on Android Studio Ladybug

From version Ladybug | 2024.2.1 of Android Studio (AS), the software ships with Java 21 as the Gradle JDK.

This causes build issues for FTC projects on SDK versions before 10.1.1.

The error looks like this:

Although its tempting to press one of those magical blue links, this is a horrible idea.

The correct fix here is to downgrade the Gradle JDK version to 17, which is fairly easy to do.

Select an option for Gradle JDK that is JDK version 17, and rebuild.

If you have no JDK 17 available, then you can click the download JDK option to install JDK 17.

JDK 17 is a good choice as it will also support the new builds for 10.1.1 onwards.

Don't upgrade the Gradle version or Android Gradle Plugin version

Consider checking out our article on alternate project setups

Android studio loves to tempt you with this little pop up, prompting you to upgrade the Android Gradle Plugin (AGP).

You need to click on [More] > Don't show again for this project. This will prevent the issue from occurring again.

These upgrades are not productive, and interfere with the current gradle build, even if you get it working on your computer, it may cause issues with team member's computers, and it makes it harder to upgrade the SDK when a new version releases. Additionally, it may cause issues when working with other gradle operations, like adding libraries.

If you accidentally upgrade the AGP or the Gradle version, you should hopefully be able to use git to undo the change, or, you can override those files with ones from the SDK.

It may be possible to undo the changes more simply by downgrading again.

It may be easier to find these files with the file explorer set to Project mode, rather than Android mode.

10.1.1 onwards

The AGP version should be 8.7.0. It is set in the project root build.gradle.

The Gradle version should be 8.9. It is set in gradle/wrapper/gradle-wrapper.properties.

Before 10.1.1

The AGP version should be 7.2.0. It is set in the project root build.gradle.

The Gradle version should be 7.4.2. It is set in gradle/wrapper/gradle-wrapper.properties.

Alternative Project Templates

Dairy hosts a series of plugins and templates that use them in order to simplify your TeamCode project structure.

Dairy-Foundation/Templates

Advantages:

• Gradle configuration is managed by a plugin, its easier to update the project outside of gradle version changes.
• Supports easily managing all of the SDK as one version.
• Supports easily adding Kotlin support to your project.
• Will support easily adding more FTC Libraries in the future.
• Supports building Library modules along with TeamCode modules.
• Simplified Project setup.

Disadvantages:

• Currently under documented.
• Not the official SDK setup, which may be confusing for inexperienced team members.
• Needs more examples and templates for more complex setups.
• Does not yet support many common FTC Libraries.

Null list error in Road Runner 1.0

Ingredients

1. A Road Runner 1.0 setup
2. Completed ForwardPushTest and LateralPushTest

The Recipe

The problem

If you have gotten through Road Runner 1.0 tuning to the ForwardRampLogger tuning step (you may also see this in LateralRampLogger or AngularRampLogger), sometimes you will get an empty list error when you press the "latest" button.

Solution

You must first run the OpMode from the Driver Station and then stop it once the robot's speed stops increasing. Finally, you can open the tuning page on your robot's Wi-Fi network, as the Road Runner docs say.

Last Updated: 2024-05-29

Complete TrajectoryBuilder Reference

Ingredients

1. A fully tuned Road Runner 1.0 setup or MeepMeep for Road Runner 1.0

The Problem

The current TrajectoryBuilder Reference in the official Road Runner 1.0 docs only has a few TrajectoryBuilder methods, and does not explain them very well in depth. This is a complete reference for more methods in the TrajectoryBuilder class for Road Runner 1.0.

TrajectoryBuilder Reference

Table of Contents

Path Primitives:

1. waitSeconds(double: seconds)
2. turn(Math.toRadians(double: angle))
3. turnTo(Math.toRadians(double: heading))
4. setTangent(double: r)
5. setReversed(boolean: reversed)
6. .strafeTo(new Vector2d(double: x, double: y)) & .strafeToConstantHeading(new Vector2d(x: double, y: double))
7. strafeToLinearHeading(new Vector2d(x, y), Math.toRadians(heading))
8. strafeToSplineHeading(new Vector2d(x, y), Math.toRadians(heading))
9. lineToX(x: double) & .lineToXConstantHeading(x: double)
10. lineToY(y: double) & .lineToYConstantHeading(y: double)
11. splineTo(new Vector2d(x, y), tangent)

Heading Primitives:

12. Tangent Heading (default)
13. Constant Heading
14. Linear Heading
15. Spline Heading

Path Primitives

The begin pose is the origin (0,0) with a heading of \( \frac{\pi}{6} \), with the exception of splineTo(new Vector2d(x, y), tangent), which has a heading of \( \frac{\pi}{2} \).

waitSeconds(double: seconds)

🚨 WARNING: 🚨
Ensure that you are using waitSeconds() and not wait(). All Java objects have a wait() function which causes the current thread to wait until another thread invokes a notify() or notifyAll() method. See further details in the Oracle JavaDoc. We don't care for this function, but it does show up in intellisense. Make sure you are using the waitSeconds() function instead of wait().

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

turn(Math.toRadians(double: angle))

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

Why Radians?
You may have noticed that we are turning by \( \frac{\pi}{6} \) degrees instead of degrees. This is because Road Runner 1.0's units are inches and radians by default. To use degrees, we can convert degrees to radians by using Java's Math.toRadians(degrees)

Example: Math.toRadians(90) converts 90 degrees to radians. 90 degrees is the same as \( \frac{\pi}{2} \) radians.

turnTo(Math.toRadians(double: heading))

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

setTangent(double: r)

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

setReversed(boolean: reversed)

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

.strafeTo(new Vector2d(double: x, double: y)) & .strafeToConstantHeading(new Vector2d(x: double, y: double))

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

.strafeToLinearHeading(new Vector2d(x, y), Math.toRadians(heading))

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

.strafeToSplineHeading(new Vector2d(x, y), Math.toRadians(heading))

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

What is the difference between spline interpolation and linear interpolation?

• Interpolation is a method of finding new data points (angle heading) in between two given data points (initial heading and final heading).
• Linear interpolation means that the robot interpolates its heading and turns at a constant, linear rate, from start to the end of the trajectory.
• Spline interpolation is the opposite, as the robot turns at a non-linear rate.

lineToX(x: double) & .lineToXConstantHeading(x: double)

🚨 WARNING: 🚨
It is HIGHLY RECOMMENDED to use .strafeTo() instead of any lineTo()'s! 🚨

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

lineToY(y: double) & .lineToYConstantHeading(y: double)

🚨 WARNING: 🚨
It is HIGHLY RECOMMENDED to use .strafeTo() instead of any lineTo()'s! 🚨

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

splineTo(new Vector2d(x, y), tangent) | Heading is \( \frac{\pi}{6} \)

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

Heading Primitives

The begin pose is the origin (0,0) with a heading of \( \frac{\pi}{2} \).

Tangent Heading (default)

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

Constant Heading

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

Linear Heading

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

Spline Heading

// Robot waits for the specified time in seconds (NOT MILLISECONDS!)
// This is a simple wait segment that is useful for running actions in between trajectories.

.waitSeconds(5)



// Robot turns counterclockwise by the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// If you see `Math.PI`, it is already in radians, and does not need `Math.toRadians()`. Degrees from 0 to 360 need to be converted to radians.
// To turn clockwise, use a negative angle.

.turn(-Math.PI / 6) // Turns clockwise by `Math.PI / 6` degrees, ending at a heading of 0 degrees
.turn(Math.PI / 6) // Turns counterclockwise by `Math.PI / 6` degrees, ending at the original heading



// Robot turns counterclockwise to the specified angle
// This turn is in radians, so you must convert your degrees to radians using `Math.toRadians()`.
// By default, the robot will turn in the shortest direction to the specified heading.
// To turn in the opposite direction, you can add or subtract a very small number (1e-6) to the heading you want to turn to.
// If it still does not work, you can use the `turn()` method instead.

.turnTo(Math.toRadians(90)) // Turns to a heading of 90 degrees
.turnTo(Math.PI / 6) // Turns to a heading of `Math.PI / 6` degrees, ending at the original heading



// `setTangent()` allows you to set a heading tangent on a trajectory, allowing you to follow a trajectory at arbitrary heading tangents
// This is equivalent to specifying a custom tangent in the `TrajectoryBuilder()` constructor.

.setTangent(90) // Sets tangent to 90



// If you see these hooks on the start and/or end of spline trajectories, you can use `setReversed()` to fix them
// These hooks make your robot move backwards instead of forward or vice versa in splines, creating suboptimal paths.
// This can be fixed by reversing the path using `setReversed(true)`.

.setReversed(false)  // Unreversed trajectory has hooks on the start and end
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



.setReversed(true)  // Reversed trajectory has no hooks on the start and end, and is smooth
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// By default, each trajectory is set to `setReversed(false)`, which does not reverse the paths.
// This means that:
.setReversed(false)
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.setReversed(false)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)

// Is the same as:
.splineTo(Vector2d(-48.0, -24.0), -Math.PI / 2)
.splineTo(Vector2d(-48.0, 0.0), Math.PI)



// Robot moves to the specified coordinates while maintaining its heading.
// Both `strafeTo()` and `strafeToConstantHeading()` are equivalent.
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

.strafeTo(new Vector2d(48, -48))
.strafeToConstantHeading(new Vector2d(48, -48))



// Robot moves to the specified coordinates while linearly interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToLinearHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified coordinates while splinely interpolating between the start heading and a specified end heading
// In other words, it constantly turns to a certain heading (once more, in radians) while moving to the specified coordinates.

.strafeToSplineHeading(new Vector2d(36, 36), Math.toRadians(90))



// Robot moves to the specified x coordinate in the direction of the robot heading (straight line).
// Both `lineToX()` and `lineToXConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToX(48)
.lineToXConstantHeading(48)



// Robot moves to the specified y coordinate in the direction of the robot heading (straight line).
// Both `lineToY()` and `lineToYConstantHeading()` are equivalent.
// 🚨 Will cause an error if your heading is perpendicular to direction your robot is traveling! 🚨

.lineToY(36)
.lineToYConstantHeading(36)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while following a tangent heading interpolator

.setTangent(0)
.splineTo(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while keeping the heading constant
// The robot maintains the heading it starts at throughout the trajectory.
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToConstantHeading(new Vector2d(48, 48), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately linearly interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToLinearHeading(new Pose2d(48, 48, 0), Math.PI / 2)



// Robot moves to the specified coordinates in a spline path while separately spline interpolating the heading
// To change the shape of the spline, change `endTangent`.

.setTangent(0)
.splineToSplineHeading(new Pose2d(48, 48, 0), Math.PI / 2)

Resources

• Official Road Runner 1.0 Builder Reference
• Official Road Runner 1.0 Builder Playground
• waitSeconds() Video Playground
• turn() Video Playground
• turnTo() Video Playground
• .setReversed(false) Video Playground
• .setReversed(true) Video Playground
• strafeTo() / strafeToConstantHeading() Video Playground
• .strafeToLinearHeading() Video Playground
• .strafeToSplineHeading() Video Playground
• .lineToX() Video Playground
• .lineToY() Video Playground
• .splineTo() Video Playground
• .splineTo() (default) Video Playground
• .splineToConstantHeading() Video Playground
• .splineToLinearHeading() Video Playground
• .splineToSplineHeading() Video Playground

Last Updated: 2024-07-29

Is the Bump On Manual Feedforward Tuner Normal?

Yes! The bump when accelerating and decelerating is normal. It is caused by a fundamental hardware issue with the Control and Expansion Hubs that makes deceleration weird. There's nothing you can do about it; just try to get the plates and slopes to match up as closely as possible.

Last Updated: 2024-05-29

Manual Feedforward Tuner Overshoots

This is normal! The REV hub motor controllers are not great at decelerating, so this typically causes about a 10% overshoot on manual feedforward tuner. It is okay to move on to the next tuning steps.

However, when you get to the feedback tuning, whether it's "Back and Forth" or "FollowerPIDTuner," you will want to add a non-zero kD term. This will help the robot not overshoot.

Last Updated: 2024-05-30

Target Velocity is Positive When Measured Velocity is Negative When Tuning Manual Feedforward

If MotorDirectionDebugger works perfectly, this means that either your right side encoders are plugged in to the wrong ports (so swap frontRight and backRight encoder cables) or your left side encoders are plugged in to the wrong ports (so swap frontLeft and backLeft encoder cables). An easy way to debug this is to add a printEncoderValues telemetry method in SampleMecanumDrive.

public void printEncoderValues(Telemetry telemetry) {
        telemetry.addData("LeftFrontPos: ", leftFront.getCurrentPosition());
        telemetry.addData("RightFrontPos: ", rightFront.getCurrentPosition());
        telemetry.addData("LeftRearPos: ", leftRear.getCurrentPosition());
        telemetry.addData("RightBackPos: ", rightRear.getCurrentPosition());
} 

Then at the end of every loop in MotorDirectionDebugger, call

drive.printEncoderValues(telemetry);

Last updated: 2024-05-30

Robot Localization Makes Circle When Spinning In Place

So when you spin the robot in place, the drawing on FTC Dashboard is making a circle. This is normal and can be fixed.

What causes it?

When the robot is spun, the strafing odometry wheel moves which makes the localization think the robot moved in a circle. This can be counteracted using the forward distance from the strafing tracking wheel to the center of rotation.

Three Wheel Solution

For three wheel odometry, this means your forward offset isn't tuned correctly. Run the tuner and replace the value. If that doesn't work, check whether the strafing pod is closer to the front or the back of the robot. If it's closer to the back, the offset should be negative.

Two Wheel Solution

The solution for two wheel odometry is largely the same. Instead of the forward offset, you must tune the x and y position of the strafing pod. The same advice about positive and negative offset still applies.

Last Updated: 2024-05-30

Robot Velocity Plateaus Below Target Velocity Plateaus

This means you've reached your robot's actual max velocity. You should lower the max velocity specified in DriveConstants. Run the MaxVelocityTuner to find the recommended max velocity to use.

Last Updated: 2024-02-08

Robot Drifts to One Side During Manual Feedforward Tuning

If this happens, you shouldn't worry. This can be caused by many reasons, such as an unbalanced robot or one wheel having slightly more friction than the others.

Whatever the reason, this will be corrected for in the later tuning steps. It can safely be ignored.

Last Updated: 2024-05-30

Robot Drifts While Tuning Follower PID

Check Localization

Run LocalizationTest and drive the robot back and forth a few times. You want to ensure that the behavior shown on the dashboard mirrors that of what you can see. If the robot was veering in BackAndForth, see if the dashboard bot is veering the same. If you notice the same discrepancy while running LocalizationTest, it means the problem is in your localization.

Tuning PID

If you're certain that localization works fine and the robot "knows" that it's wrong, but isn't correcting, then you need to tune your PID values more. You can tune your controller through the steps on this page of Game Manual 0.

Last Updated: 2024-01-21

Robot Drive Full Speed on Start When Following Trajectory

If you are running an OpMode that has Roadrunner trajectories in it, and when you start moving it goes at full speed right away, this almost always means you forgot to set a pose estimate. In init(), before you run any trajectories, make sure you have drive.setPoseEstimate(startingPose), whatever your starting pose may be. Make sure that the trajectory starting pose matches this:

Trajectory traj = drive.trajectoryBuilder(startingPose)
    ...
    .build();

Last Updated: 2024-01-20

How to Integrate a PIDF Controller With Roadrunner

This recipe will assume you have a functioning PIDF controller that has already been tuned. If you do not, refer to integrating a custom PIDF controller.

Ingredients

1. A PID or PIDF controller class
2. Tuned PID(F) gains
3. An OpMode or LinearOpMode
4. A Finite State Machine

The Recipe

PID(F) Controller and gains

This recipe assumes you have 1) a PID(F) class that works and 2) tuned PID(F) gains. This recipe will not go over how to implement these; you should reference integrating a custom PIDF controller.

Finite State Machines

In short, a finite state machine is a code structure which allows code to run linearly while also having quasi-parallel actions running. The example we will be working with today is driving with Roadrunner while controlling linear slides. For a more indepth understanding of what finite state machines are, visit gm0.

You can work with Finite State Machines in either a LinearOpMode or an OpMode, either work. For this recipe, we will be using a LinearOpMode. To use an OpMode, move everything before the while loop into the init() function and everything in the while loop into the loop() function.

We will first have a full example and then break it down piece by piece.

This example is more like pseudocode than real code and is meant to demonstrate a methodology.

public class RoadRunnerPIDF extends LinearOpMode {
    // the capitalization and snake_case is just convention because the values of an enum are constants
    public enum STATES {
        INIT,
        DRIVE_FORWARD,
        STRAFE_LEFT_AND_LIFT_SLIDES,
        DRIVE_BACKWARD,
        STOP;
    }

    private STATES previousState = STATES.INIT;
    private STATES currentState = STATES.INIT;
    private int targetPosition = 0;

    private TrajectorySequence forward;
    private TrajectorySequence strafeLeft;
    private TrajectorySequence backward;

    private SampleMecanumDrive drive;
    private DcMotorEx linearSlides;
    private PIDFController PIDF;

    public void runOpMode() {
        /* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
        The linear slides will simply be called linearSlides.
        */

        // linear slide initialization code
        // pidf initialization code

        drive = new SampleMecanumDrive(hardwareMap);
        drive.setPoseEstimate(new Pose2d());

        forward = drive.TrajectorySequenceBuilder(new Pose2d())
            .forward(10)
            .build();

        strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
            .addDisplacementMarker(() -> {
                targetPosition = 800;
            })
            .strafeLeft(10)
            .build();

        backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
            .back(10)
            .build();

        waitForStart();

        currentState = STATES.DRIVE_FORWARD;

        while(opModeIsActive) {
            switch (currentState) {
                case (INIT):
                    break;
                case (DRIVE_FORWARD):
                    if (previousState != currentState) {
                        // everything in here will run once when the state switches

                        drive.followTrajectoryAsync(forward)
                        previousState = STATES.DRIVE_FORWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
                    }
                    break;
                case (STRAFE_LEFT_AND_LIFT_SLIDES):
                    if (previousState != currentState) {
                        // inside this trajectory sequence the targetPosition is set and the slides will start updating
                        drive.followTrajectorySequenceAsync(strafeLeft);
                    } else if (!drive.isBusy() && linearSlides.atTarget()) {
                        currentState = STATES.DRIVE_BACKWARD;
                    }
                    break;
                case (DRIVE_BACKWARD):
                    if (previousState != currentState) {
                        drive.followTrajectorySequenceAsync(backward);
                        previousState = STATES.DRIVE_BACKWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STOP;
                    }
                    break;
                case (STOP):
                    break;
            }

            // outside of the switch we update our slides, that way they are always receiving new information
            drive.update();
            double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
            linearSlides.setPower(power);
        }
    }
}

Okay, let's break this down piece by piece. First, what is an "enum" and why do we use them? Enums are a way to define a set of named constant values. They provide a convenient and readable way to work with predefined, named values in your code. Here, we used an enum to describe the various states the robot could be in.

public class RoadRunnerPIDF extends LinearOpMode {
    // the capitalization and snake_case is just convention because the values of an enum are constants
    public enum STATES {
        INIT,
        DRIVE_FORWARD,
        STRAFE_LEFT_AND_LIFT_SLIDES,
        DRIVE_BACKWARD,
        STOP;
    }

    private STATES previousState = STATES.INIT;
    private STATES currentState = STATES.INIT;
    private int targetPosition = 0;

    private TrajectorySequence forward;
    private TrajectorySequence strafeLeft;
    private TrajectorySequence backward;

    private SampleMecanumDrive drive;
    private DcMotorEx linearSlides;
    private PIDFController PIDF;

    public void runOpMode() {
        /* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
        The linear slides will simply be called linearSlides.
        */

        // linear slide initialization code
        // pidf initialization code

        drive = new SampleMecanumDrive(hardwareMap);
        drive.setPoseEstimate(new Pose2d());

        forward = drive.TrajectorySequenceBuilder(new Pose2d())
            .forward(10)
            .build();

        strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
            .addDisplacementMarker(() -> {
                targetPosition = 800;
            })
            .strafeLeft(10)
            .build();

        backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
            .back(10)
            .build();

        waitForStart();

        currentState = STATES.DRIVE_FORWARD;

        while(opModeIsActive) {
            switch (currentState) {
                case (INIT):
                    break;
                case (DRIVE_FORWARD):
                    if (previousState != currentState) {
                        // everything in here will run once when the state switches

                        drive.followTrajectoryAsync(forward)
                        previousState = STATES.DRIVE_FORWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
                    }
                    break;
                case (STRAFE_LEFT_AND_LIFT_SLIDES):
                    if (previousState != currentState) {
                        // inside this trajectory sequence the targetPosition is set and the slides will start updating
                        drive.followTrajectorySequenceAsync(strafeLeft);
                    } else if (!drive.isBusy() && linearSlides.atTarget()) {
                        currentState = STATES.DRIVE_BACKWARD;
                    }
                    break;
                case (DRIVE_BACKWARD):
                    if (previousState != currentState) {
                        drive.followTrajectorySequenceAsync(backward);
                        previousState = STATES.DRIVE_BACKWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STOP;
                    }
                    break;
                case (STOP):
                    break;
            }

            // outside of the switch we update our slides, that way they are always receiving new information
            drive.update();
            double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
            linearSlides.setPower(power);
        }
    }
}

By using names with meaning like these, it is much clearer when writing and reading the code what each block does. It also means we don't have to remember that state 0 means START and state 1 means DRIVE_FORWARD, etc.

Next, we initialize everything and build our trajectories. The important one to note is creating strafeLeft, which includes slide movement.

public class RoadRunnerPIDF extends LinearOpMode {
    // the capitalization and snake_case is just convention because the values of an enum are constants
    public enum STATES {
        INIT,
        DRIVE_FORWARD,
        STRAFE_LEFT_AND_LIFT_SLIDES,
        DRIVE_BACKWARD,
        STOP;
    }

    private STATES previousState = STATES.INIT;
    private STATES currentState = STATES.INIT;
    private int targetPosition = 0;

    private TrajectorySequence forward;
    private TrajectorySequence strafeLeft;
    private TrajectorySequence backward;

    private SampleMecanumDrive drive;
    private DcMotorEx linearSlides;
    private PIDFController PIDF;

    public void runOpMode() {
        /* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
        The linear slides will simply be called linearSlides.
        */

        // linear slide initialization code
        // pidf initialization code

        drive = new SampleMecanumDrive(hardwareMap);
        drive.setPoseEstimate(new Pose2d());

        forward = drive.TrajectorySequenceBuilder(new Pose2d())
            .forward(10)
            .build();

        strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
            .addDisplacementMarker(() -> {
                targetPosition = 800;
            })
            .strafeLeft(10)
            .build();

        backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
            .back(10)
            .build();

        waitForStart();

        currentState = STATES.DRIVE_FORWARD;

        while(opModeIsActive) {
            switch (currentState) {
                case (INIT):
                    break;
                case (DRIVE_FORWARD):
                    if (previousState != currentState) {
                        // everything in here will run once when the state switches

                        drive.followTrajectoryAsync(forward)
                        previousState = STATES.DRIVE_FORWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
                    }
                    break;
                case (STRAFE_LEFT_AND_LIFT_SLIDES):
                    if (previousState != currentState) {
                        // inside this trajectory sequence the targetPosition is set and the slides will start updating
                        drive.followTrajectorySequenceAsync(strafeLeft);
                    } else if (!drive.isBusy() && linearSlides.atTarget()) {
                        currentState = STATES.DRIVE_BACKWARD;
                    }
                    break;
                case (DRIVE_BACKWARD):
                    if (previousState != currentState) {
                        drive.followTrajectorySequenceAsync(backward);
                        previousState = STATES.DRIVE_BACKWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STOP;
                    }
                    break;
                case (STOP):
                    break;
            }

            // outside of the switch we update our slides, that way they are always receiving new information
            drive.update();
            double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
            linearSlides.setPower(power);
        }
    }
}

We used a displacement marker, which tells Roadrunner to run this code at the specified position along the trajectory. The () -> {} is the lambda format for a one time use function. The empty parentheses indicate that the function requires no arguments, and the curly braces denote the start of the function. In this case, we're just setting the targetPosition variable, but this marker could include setting servo position, reading sensors, or anything else really.

public class RoadRunnerPIDF extends LinearOpMode {
    // the capitalization and snake_case is just convention because the values of an enum are constants
    public enum STATES {
        INIT,
        DRIVE_FORWARD,
        STRAFE_LEFT_AND_LIFT_SLIDES,
        DRIVE_BACKWARD,
        STOP;
    }

    private STATES previousState = STATES.INIT;
    private STATES currentState = STATES.INIT;
    private int targetPosition = 0;

    private TrajectorySequence forward;
    private TrajectorySequence strafeLeft;
    private TrajectorySequence backward;

    private SampleMecanumDrive drive;
    private DcMotorEx linearSlides;
    private PIDFController PIDF;

    public void runOpMode() {
        /* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
        The linear slides will simply be called linearSlides.
        */

        // linear slide initialization code
        // pidf initialization code

        drive = new SampleMecanumDrive(hardwareMap);
        drive.setPoseEstimate(new Pose2d());

        forward = drive.TrajectorySequenceBuilder(new Pose2d())
            .forward(10)
            .build();

        strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
            .addDisplacementMarker(() -> {
                targetPosition = 800;
            })
            .strafeLeft(10)
            .build();

        backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
            .back(10)
            .build();

        waitForStart();

        currentState = STATES.DRIVE_FORWARD;

        while(opModeIsActive) {
            switch (currentState) {
                case (INIT):
                    break;
                case (DRIVE_FORWARD):
                    if (previousState != currentState) {
                        // everything in here will run once when the state switches

                        drive.followTrajectoryAsync(forward)
                        previousState = STATES.DRIVE_FORWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
                    }
                    break;
                case (STRAFE_LEFT_AND_LIFT_SLIDES):
                    if (previousState != currentState) {
                        // inside this trajectory sequence the targetPosition is set and the slides will start updating
                        drive.followTrajectorySequenceAsync(strafeLeft);
                    } else if (!drive.isBusy() && linearSlides.atTarget()) {
                        currentState = STATES.DRIVE_BACKWARD;
                    }
                    break;
                case (DRIVE_BACKWARD):
                    if (previousState != currentState) {
                        drive.followTrajectorySequenceAsync(backward);
                        previousState = STATES.DRIVE_BACKWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STOP;
                    }
                    break;
                case (STOP):
                    break;
            }

            // outside of the switch we update our slides, that way they are always receiving new information
            drive.update();
            double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
            linearSlides.setPower(power);
        }
    }
}

So now we're getting to the Finite State Machine (FSM) part. The first part of this case, which you'll see in each part, is checking whether previous and current states are equal. This allows us to run code the first time it enters this state, like starting a trajectory (in this example). Then inside that same block, we also need to set the previous state to the one we're in.

The else if just checks if we're done with this state to detect when to move on. This is the transition trigger. In this case, it detects when the Roadrunner trajectory finishes.

The next case is the more interesting one.

public class RoadRunnerPIDF extends LinearOpMode {
    // the capitalization and snake_case is just convention because the values of an enum are constants
    public enum STATES {
        INIT,
        DRIVE_FORWARD,
        STRAFE_LEFT_AND_LIFT_SLIDES,
        DRIVE_BACKWARD,
        STOP;
    }

    private STATES previousState = STATES.INIT;
    private STATES currentState = STATES.INIT;
    private int targetPosition = 0;

    private TrajectorySequence forward;
    private TrajectorySequence strafeLeft;
    private TrajectorySequence backward;

    private SampleMecanumDrive drive;
    private DcMotorEx linearSlides;
    private PIDFController PIDF;

    public void runOpMode() {
        /* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
        The linear slides will simply be called linearSlides.
        */

        // linear slide initialization code
        // pidf initialization code

        drive = new SampleMecanumDrive(hardwareMap);
        drive.setPoseEstimate(new Pose2d());

        forward = drive.TrajectorySequenceBuilder(new Pose2d())
            .forward(10)
            .build();

        strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
            .addDisplacementMarker(() -> {
                targetPosition = 800;
            })
            .strafeLeft(10)
            .build();

        backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
            .back(10)
            .build();

        waitForStart();

        currentState = STATES.DRIVE_FORWARD;

        while(opModeIsActive) {
            switch (currentState) {
                case (INIT):
                    break;
                case (DRIVE_FORWARD):
                    if (previousState != currentState) {
                        // everything in here will run once when the state switches

                        drive.followTrajectoryAsync(forward)
                        previousState = STATES.DRIVE_FORWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
                    }
                    break;
                case (STRAFE_LEFT_AND_LIFT_SLIDES):
                    if (previousState != currentState) {
                        // inside this trajectory sequence the targetPosition is set and the slides will start updating
                        drive.followTrajectorySequenceAsync(strafeLeft);
                    } else if (!drive.isBusy() && linearSlides.atTarget()) {
                        currentState = STATES.DRIVE_BACKWARD;
                    }
                    break;
                case (DRIVE_BACKWARD):
                    if (previousState != currentState) {
                        drive.followTrajectorySequenceAsync(backward);
                        previousState = STATES.DRIVE_BACKWARD;
                    } else if (!drive.isBusy()) {
                        currentState = STATES.STOP;
                    }
                    break;
                case (STOP):
                    break;
            }

            // outside of the switch we update our slides, that way they are always receiving new information
            drive.update();
            double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
            linearSlides.setPower(power);
        }
    }
}

Here we have the same structure. However, this time our transition trigger is finishing the Roadrunner trajectory and the linear slides reaching their target. Because this runs in a loop, once the displacement marker triggers and changes the targetPosition, the PID update that runs at the end of every loop will move the linear slides accordingly.

It is also important to note that when using async following, you must call drive.update() once every loop. This allows Roadrunner to track the robot's movement and to ensure the motors are following the trajectory. Without it, the robot will not move.

Whew! You should now be able to integrate a PID(F) controller with Roadrunner trajectories.

This example was meant to be general and explain the structure and concepts needed to make PID(F) controllers work with Roadrunner. It will almost certainly require changes to make it work exactly how you wish, so don't worry if your code doesn't look exactly like this example!

State Factory

State Factory is a library which helps abstract a lot of the code of a finite state machine. It also helps ensure you don't forget to write a break or an exit case.

This recipe will not cover the installation of State Factory. Please follow the instructions on their gitbook to install it.

So, we're going to write the same finite state machine but this time using State Factory.

public class RoadRunnerPIDFSF extends LinearOpMode {

    public enum STATES {
        INIT,
        DRIVE_FORWARD,
        STRAFE_LEFT_AND_LIFT_SLIDES,
        DRIVE_BACKWARD,
        STOP;
    }

    SampleMecanumDrive drive;
    DcMotorEx linearSlides;
    PIDFController PIDF;
    int targetPosition = 0;

    public void runOpMode() {

        // all the same initialization and trajectory building as above

        StateMachine machine = new StateMachine()
            .state(STATES.INIT) // creates a new state
            .transition(() -> isStarted()) // condition to transition from this state to the next one

            .state(STATES.DRIVE_FORWARD) // register a new state
            .onEnter(() -> drive.followTrajectorySequenceAsync(forward)) // code to happen one time when entering this state
            .transition(() -> !drive.isBusy())

            .state(STATES.STRAFE_LEFT_AND_LIFT_SLIDES)
            .onEnter(() -> drive.followTrajectorySequenceAsync(strafeLeft))
            .transition(() -> (!drive.isBusy() && linearSlides.atTarget()))

            .state(STATES.DRIVE_BACKWARD)
            .onEnter(() -> drive.followTrajectorySequenceAsync(backward))
            .transition(() -> !drive.isBusy())

            .state(STATES.STOP)

            .build();
        // building this StateMachine doesn't actually do anything. We still need to run it

        waitForStart();

        machine.start(); // this starts the state machine, putting us into the first state

        while(opModeIsActive()) {
            machine.update();
            drive.update();
            double power = PIDF(linearSlides.getCurrentPosition(), targetPosition);
            linearSlides.setPower(power);
        }
    }
}

These two examples both do the exact same thing. This introduction to State Factory was mostly meant to show how it can simplify writing FSMs.

Android studio may recommend changing something like () -> !drive.isBusy() to !drive::isBusy. These are simply two different ways to write the same thing. The double colon works like class/instance::method.

It is important to note that these were extremely simple FSMs and do not demonstrate their full capabilities. This was simply meant to show you a way to integrate RoadRunner and a PIDF controller.

Last Updated: 2024-01-23

NullPointerException on initialization

NullPointerExceptions are common errors that occur when code runs in the wrong order. Here, we will cover how and why NPEs happen in general, as well as some common FTC-specific issues.

Ingredients

1. A limited knowledge of Java.
2. A desire to learn, debug, or solve problems!

Quick Links: Common Issues

Hardware Devices in OpModes Hardware Devices in External Classes BlocksCompanion hardwareMap and telemetry

The Recipe

What does it look like?

The general format of the error is:

java.lang.NullPointerException: Attempt to invoke [...] on a null object reference

On the Driver Station, you may see a stacktrace similar to this:

Why does this happen?

First, we need to understand more about how Java works a little more in depth.

This section is a little lengthy, feel free to scroll below for the solution.

When we program in Java, we have expressions, which have a certain type. The type tells us about the properties of said expression.

This lets us add ints, set the power of a DcMotor, or check if a boolean is true!

Type systems also give us a degree of validity; we can't add servos to booleans.

The following is a very generalized description with oversights, but is sufficient for conceptual understanding.

In Java, there are two categories of types:

• Primitive Types
  • Primitives are not objects, and do not have methods, only a value.
  • Primitives are passed by value.
  • int, double, and boolean, are examples of primitives.
• Reference Types
  • All types that extend Object are passed by reference, and hence, reference types.
  • All objects are passed by reference.
  • classes, interfaces, enums, arrays

What does it mean to pass/store an object by value or reference?

Storing by Value:

• You are storing the actual value of the variable in memory.
• This means that when you assign one variable to another, a copy of the value is made.
• Changes to one variable do not affect the other.

• int bobMoney = 20;
  int jeffMoney = bobMoney; // "jeffMoney" gets the value of "bobMoney", not a reference to "bobMoney"
  
  jeffMoney = 10; // changing "jeffMoney" does not affect "bobMoney"
  
  System.out.println(bobMoney); // 20
  

Storing by Reference:

• You are storing a reference or memory address to the location where the actual data is stored.
• This means that when you assign one variable to another, they both point to the same memory location.
• Changes to one variable will affect the other because they both refer to the same data.

• Person bob = new Person("bob", 18);
  Person anon = bob; // "anon" now refers to the same object as "bob"
  
  System.out.println(bob.getAge()); // will print 18
  
  anon.setAge(21); // changing "anon" also changes "bob"
  
  System.out.println(bob.getAge()); // will print 21
  

null really refers to a null reference. This means any Object can have a null value.

Any uninitialized Object has no reference to point to; a null reference, or null. NPEs occur when you try to use the typed properties of an object while it points to nothing. This is so no undefined behavior occurs.

Java does not provide any means of "null-safety", and so it is the responsibility of the programmer to check for and handle potential null values.

FTC specific examples include trying to access the hardwareMap at instantiation, or just never assigning a value to a HardwareDevice.

hardwareMap and telemetry

It is important to note that NPEs are a very common, generic exception. However, the most common causes in FTC are due to the way hardwareMap and telemetry work. This section will detail the way that hardwareMap and telemetry work, why it's so easy to get them wrong, and how to fix it.

Whenever you write an OpMode, you use the telemetry and hardwareMap objects all the time!

// Instantiate drive motors
DcMotor frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");

A generic example of using hardwareMap.

However, these objects have to come from somewhere, and in fact they do! They are both created within the OpMode itself. This means that you cannot directly access hardwareMap OR telemetry from outside an OpMode. This is a very common issue, and typically the biggest cause of NullPointerExceptions.

There is actually even another layer to this - while telemetry can be accessed anywhere within the OpMode, hardwareMap must only be accessed after the OpMode has started running.

Technical details: hardwareMap and telemetry are both from OpModeInternal (which both OpMode and LinearOpMode inherit). telemetry is instantiated on class construction, whereas hardwareMap is instantiated as soon as the OpMode is run.

Common Issues: Hardware Devices in OpModes

Hardware devices (anything that requires hardwareMap) are NOT accessible at class instantiation; that is, one cannot do the following:

@TeleOp
public class Testing extends OpMode {
  private DcMotor motor = hardwareMap.get(DcMotor.class, "motor"); // this will cause a NullPointerException because hardwareMap isn't defined until init()!

  @Override
  public void init() { } // it's always a red flag if the init is empty! TH=

  @Override
  public void loop() { 
    motor.setPower(1.0); 
  }
}

Hardware devices only start to become accessible during and after init() in OpModes and within runOpMode() in LinearOpModes. Therefore, if you're using an OpMode, you should be instantiating (creating) your hardware devices in init(). In a LinearOpMode, hardware devices should be instatiated in runOpMode(), and before waitForStart().

@TeleOp
public class Testing extends OpMode {
  private DcMotor motor;

  @Override
  public void init() {
    motor = hardwareMap.get(DcMotor.class, "motor"); // hardwareMap is defined here, so this won't cause an error!
    // any other hardware device instantiations should also go here
  }

  @Override
  public void loop() {
    motor.setPower(1.0); 
  }
}

Common Issues: Hardware Devices in External Classes

Let's say you have a separate class where you want to access either hardwareMap or telemetry, or both. For example, we might have an Arm class that controls a simple rotating arm.

public class Arm {
  private DcMotor armMotor;

  /**
   * Tilts the arm using raw motor power.
   * @param power The motor power.
   */
  public void tilt(double power) {
    armMotor.setPower(power);
  }
}

Now, you might notice in the above code that the value of armMotor is never set to anything. This will cause a NullPointerException! To prevent this, we need to assign armMotor a value. Typically, we'd do this using hardwareMap. However, if you recall, hardwareMap isn't defined outside of OpModes! So, what to do? Well, the idea is actually rather simple: when creating an Arm, we'll ask for an instance of hardwareMap that we can then use to define armMotor. Since Arm is created in an OpMode, hardwareMap will be defined!

public class Arm {
  private DcMotor armMotor;

  /**
   * @param hardwareMap The hardwareMap instance from an OpMode.
   */ 
  public Arm(HardwareMap hardwareMap) {
    armMotor = hardwareMap.get(DcMotor.class, "armMotor");
  }

  /**
   * Tilts the arm using raw motor power.
   * @param power The motor power.
   */
  public void tilt(double power) {
    armMotor.setPower(power);
  }
}

Now that we have our Arm class, this is what an OpMode would look like:

@TeleOp
public class ArmTest extends OpMode {
  private Arm arm;

  @Override
  public void init() {
    arm = new Arm(hardwareMap); // we still need to instantiate the arm in init(), since hardwareMap isn't defined before then
  }

  @Override
  public void loop() {
    arm.tilt(gamepad1.right_trigger - gamepad1.left_trigger); // use triggers to move arm up and down
  }
}

We can also do the same thing for telemetry:

private Telemetry telemetry;

public Arm(HardwareMap hardwareMap, Telemetry opModeTelemetry) {
  armMotor = hardwareMap.get(DcMotor.class, "armMotor");
  telemetry = opModeTelemetry;
}

...and we now have an Arm class that can access both hardwareMap and telemetry!

Common Issues: BlocksCompanion hardwareMap and telemetry

Remember when we mentioned how hardwareMap and telemetry aren't defined outside of an OpMode? While this is true, there is still a way you can make your code think it can access hardwareMap and telemetry. Doing this will cause all of the NullPointerException problems with none of the helpful red squiggly lines.

package org.firstinspires.ftc.teamcode;

import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry;

import com.qualcomm.robotcore.hardware.DcMotor;

public class Arm {
  private DcMotor armMotor;

  public Arm() {
    armMotor = hardwareMap.get(DcMotor.class, "armMotor");
  }

  /**
   * Tilts the arm using raw motor power.
   * @param power The motor power.
   */
  public void tilt(double power) {
    armMotor.setPower(power);
    telemetry.addData("Arm tilt power", power); // log tilt power in telemetry
  }
}

What's wrong here? You might notice how our constructor for Arm no longer has a parameter for hardwareMap, yet we can still use hardwareMap somehow! Similarly, we are also using telemetry without ever even defining it! This bug is incredibly sneaky. Normally, we don't pay a ton of attention to imports, but here the imports are exactly what matter. Let's isolate the important imports:

import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry;

These two imports are incredibly nasty. The hardwareMap and telemetry above are not actually meant for us, but actually for Blocks users! This is, behind the scenes, what Blocks OpModes use. Since we are not using Blocks, these imports don't work. Luckily, although the bug is nasty, the solution is rather simple - get rid of the imports and pass hardwareMap and telemetry in like we did in the previous section!

Last updated: 2024-11-27

Integrating a Custom PID(F) Controller

This recipe does not cover usage for Roadrunner or Command-Based structures.

PID(F) controllers are some of the most used controllers in FTC. However, it can be confusing and challenging to properly integrate them into your OpModes. This recipe will go over an example of how to integrate a PID(F) controller alongside a manual control system.

Ingredients

1. A PID or PIDF controller class (this should be a file that is something like PIDFController.java, or you may use a pre-made one from a library like FTCLib).
2. A use case for the PID(F).
3. An OpMode or LinearOpMode.

The Recipe

Creating a PID(F) Controller

The first part of using a PID(F) controller is creating one. To do this, we need to declare the PID(F) controller within the OpMode:

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; // This example is for a LinearOpMode, though a similar idea applies to regular OpModes.
import org.firstinspires.ftc.teamcode.controllers.PIDController; // This may vary depending on what implementation you are using.
import org.firstinspires.ftc.teamcode.controllers.PIDFController; // This may vary depending on what implementation you are using.

@TeleOp
public class ExampleOpMode extends LinearOpMode {

    // This line creates a PIDF controller named examplePIDF that has coefficients of:
    // kP = 0
    // kI = 0
    // kD = 0
    // kF = 0
    private PIDFController examplePIDF = new PIDFController(0, 0, 0, 0);

    // This line creates a PID controller named examplePID that has coefficients of:
    // kP = 0
    // kI = 0
    // kD = 0
    private PIDController examplePID = new PIDController(0, 0, 0);

    @Override
    public void runOpMode() {
        // OpMode code goes here
    }

}

Now that we have our PID(F) controller, we need to use it! One of the most common use cases for a PID(F) controller is moving a motor to a certain motor encoder position. As an example, let's say we have a linear slide, and want to move it to 500 ticks when we press "a." We also want to be able to move it up and down using the triggers. The following code is for a LinearOpMode (the while (opModeIsActive()) section would go in the loop() function for a OpMode):

public void runOpMode() {
    // Put all of your initialization here.
    DcMotor slides = hardwareMap.dcMotor.get("slides");
    waitForStart();

    int targetPosition = 500;

    // We will use this variable to determine if we want the PIDF to run.
    boolean usePIDF = false;

    Gamepad lastGamepad1 = new Gamepad();
    Gamepad lastGamepad2 = new Gamepad();

    while (opModeIsActive()) {

        // This is a rising-edge detector that runs if and only if "a" was pressed this loop.
        if (gamepad1.a && !lastGamepad1.a) {
            usePIDF = true;
        }


        if (gamepad1.left_trigger > 0) {
            slides.setPower(gamepad1.left_trigger);

            // If we get any sort of manual input, turn PIDF off.
            usePIDF = false;
        } else if (gamepad1.right_trigger > 0) {
            slides.setPower(gamepad1.right_trigger);

             // If we get any sort of manual input, turn PIDF off.
            usePIDF = false;
        } else if (usePIDF) {
            // Sets the slide motor power according to the PIDF output.
            slides.setPower(examplePIDF.calculate(slides.getCurrentPosition(), targetPosition));
        }
    }
}

This is a lot, so let's break it down piece by piece. First, we initialize our slide motor, which we call slides.

DcMotor slides = hardwareMap.dcMotor.get("slides");

Next, we wait for the program to start and declare some variables.

waitForStart();

int targetPosition = 500;

// We will use this variable to determine if we want the PIDF to run.
boolean usePIDF = false;

Gamepad lastGamepad1 = new Gamepad();
Gamepad lastGamepad2 = new Gamepad();

targetPosition is simply the position we want the slides to go to, which is 500. usePIDF stores the state of our system, i.e. whether we want to run the PIDF or use manual control. lastGamepad1 and lastGamepad2 are used for Rising Edge Detectors. In short, they detect when a button begins to be pressed, and ignore when it is held.

The next part is the while loop, which ensures that the code runs in a loop until the OpMode stops.

while (opModeIsActive())

We then use a Rising Edge Detector to check if "a" was just pressed. If it was, we set usePIDF to true to tell the program to move to the target position.

// This is a rising-edge detector that runs if and only if "a" was pressed this loop.
if (gamepad1.a && !lastGamepad1.a) {
    usePIDF = true;
}

The next part is a little complicated, but the idea is that we only want to call slide.setPower() once, so we group all the ways it can be called together so that they can't happen at the same time.

First, we check if the left trigger is pressed. If it is, we set the slide power to an appropriate value and switch to manual control mode by setting usePIDF to false.

if (gamepad1.left_trigger > 0) {
    slides.setPower(gamepad1.left_trigger);

    // If we get any sort of manual input, turn PIDF off.
    usePIDF = false;
}

Next, we do the same check, but for the right trigger.

else if (gamepad1.right_trigger > 0) {
    slides.setPower(gamepad1.right_trigger);

        // If we get any sort of manual input, turn PIDF off.
    usePIDF = false;
}

Note that we use else to only run this code if the left trigger is not pressed. This prevents pressing both triggers at the same time from causing any issues.

Tip: If your triggers return nonzero values even when they are not being pressed, you can increase the minimum value (the 0 in the statement if (gamepad1.left_trigger > 0)) from 0 to something like 0.1.

Finally, if there are no manual inputs, and we are in the PID(F) state, we run the PID(F).

else if (usePIDF) {
    // Sets the slide motor power according to the PIDF output.
    slides.setPower(examplePIDF.calculate(slides.getCurrentPosition(), targetPosition));
}

This is a pretty standard way of using the PID(F) output to set a motor power. slides.getCurrentPosition(), as the name implies, just returns the current slide position, in ticks. The FTCLib PID(F) assumes that the first input of the function is the place where your motor is, and the second input is the place where your motor wants to be. We will be using the FTCLib PID(F) syntax here for the sake of having some standard, but either way works.

If you've read through this entire thing, then congrats! You should now have a fully functioning PID(F) controller that you can implement anywhere, even in conjunction with manual control.

Note that the example we went through is just one way PID(F)s can be used, and there are many ways to achieve this result. Don't worry if your code doesn't look exactly like this example!

As an aside, the technique we used to make sure the PID(F) control and manual control did not interfere is a simple version of what's known as a Finite State Machine. This idea of having multiple possible states and only running one at a time to ensure they don't interfere can be used for much more complex systems, such as controlling an entire Autonomous!

Best of luck with your code!

Last updated: 2024-05-29

Syncing Two Linear Slide Motors Using a PIDF Controller

Ingredients

1. A PID(F) controller
2. Tuned PID(F) coefficients

The Recipe

The Problem

Linear slides powered by two different motors can end up twisted, with one slide higher than the other. This can happen if the two encoders get out of sync. If you are using the RUN_TO_POSITION motor mode, this causes one slide to be supplied more power than the other. Even if you are using a custom PID(F) controller on each motor, the same problem would occur.

Methodology

The BEST way to keep these in sync is to have them be mechanically connected with a bar or a piece of channel. If the two act as one rigid body, then it is a lot less dependent on software. However, this is not always possible, hence the software solutions.

Instead, the way this recipe will explain is with a leader and a follower linear slide motor. This means we will use a PID(F) controller on one of the linear slide positions and just set that power to both motors. This resolves the issue of the two linear slides being supplied different powers. If both motors are going the same speed and both linear slides are well tensioned, the linear slides should stay synced.

Code Example

This code example is going to assume you have a working PID(F) controller class and tuned coefficients.

public class PIDFExample extends LinearOpMode {

    // PID(F) declaration
    // kp = 0, ki = 0, kd = 0, kf = 0;

    private PIDFController examplePIDF = new PIDFController(0, 0, 0, 0);

    @Override
    public void runOpMode() {
        int targetPosition = 0;

        // motor setup
        DcMotorEx leftSlide = hardwareMap.get(DcMotorEx.class, "leftSlide");
        DcMotorEx rightSlide = hardwareMap.get(DcMotorEx.class, "rightSlide");

        waitForStart();

        while(opModeIsActive()) {
            /* 
            Calculates PID based only on one encoder.
            This can also be the average position of the two linear slides, but we haven't noticed much difference
            */
            double power = PIDFController.update(leftSlide.getCurrentPosition(), targetPosition);

            // see how both motors are getting the same power
            leftSlide.setPower(power);
            rightSlide.setPower(power);
        }
    }

}

Last Updated: 2024-05-29

Why to only use USB 3.0

Ingredients

1. A Control Hub
2. At least 1 USB device
3. A USB hub (optional)

The Recipe

Overview

The Control Hub has a lot of nuances that many people do not know of, including the dangers of the onboard USB 2.0 port.

The Problem

The Control Hub's USB 2.0 port shares a ground with the Wi-Fi chipset on the Control Hub. This provides a path for a static shock to the USB device to cause a Wi-Fi disconnect mid-match.

How to Mitigate the Problem

The best way to mitigate the problem is to not use the USB 2.0 port on your Control Hub. For teams with no or only one USB device, that's not a problem, just use the USB 3.0 port instead of the USB 2.0 port. If your team needs more than one device, such as an Expansion Hub over USB and a USB camera for object detection, it gets more complicated. To prevent shock, you can get a USB hub and connect all devices through just the USB 3.0 port.

Last Updated: 2024-05-29

How to wire odometry pods

Ingredients

1. An Expansion Hub or Control Hub
2. either 2 or 3 odometry pods/modules

The Recipe

The problem

The Rev Control Hub and Expansion Hub, as found here by 7244 alum Eeshwar, only have 2 reliable quadrature encoder ports. This means high CPR/PPR encoders such as the Rev Through Bore encoder will miss counts on ports 1 and 2 which will lead to drift.

Solutions

2 Wheel Odometry

For teams that use a 2 wheel + IMU setup, the solution is simple! Put the drive motors on the Control Hub. Then, as you don't need drive encoders, you can simply put the odometry on Control Hub encoder ports 0 and 3 where you would usually put the motor encoders.

3 Wheel Odometry

For teams with 3 wheel odometry, it is a bit more complex. The most important odometry pods are the parallel ones since encoder drift with them will cause heading drift. This can rapidly ruin your localization as heading is used as a basis for all other measurements. Since they're the most important, the parallel pods should go in ports 0 and 3 on the Control Hub. The perpendicular (strafe) pod is less important to localization, so it is fine to put it in port 1 or 2 on the Control Hub.

Note that you should always put odometry on the Control Hub (or at least all on the same hub) even if you must place it in ports 1 or 2. This is because reading from the Expansion Hub requires an additional bulk read. This can greatly worsen loop times and is not worth the benefits of using the 0 and 3 ports.

Last Updated: 2024-05-29

Why do some FTC programmers use Kotlin? Should I switch?

Have you ever seen Kotlin mentioned in the context of FTC code?

Are you curious about why some FTC programmers like to use Kotlin for their code bases?

Kotlin is a language with very high cross-compatability with Java, which means it can be used to write your FTC code.

FIRST provides official instructions for adding Kotlin to your project here.

Ingredients

1. Good understanding of Java
2. Interest in learning and exploring Kotlin

The Recipe

Kotlin is a language that makes a very solid attempt at modernizing Java. It makes writing common Java patterns concise. Kotlin also makes it easy to write safer code that is less likely to have strange bugs or throw confusing NullPointerExceptions.

Kotlin is unlikely to be particularly useful to you if you are not using Object-Oriented aspects of Java already. If you are just writing [Linear]OpModes but are not writing your own classes, Kotlin is probably not for you. While Kotlin certainly does offer some nice features in this environment, the challenges that come with using Kotlin may also prove hard to overcome unless you are writing more complex and involved code. It is also advisable not to try to switch to Kotlin at the same time as learning more Object-Oriented skills.

Due to Kotlin's concise nature, it can sometimes prove difficult to read. Java likes to put everything out in the open and be very direct and specific, while Kotlin tends to imply much more.

This recipe will cover some basics of Kotlin syntax with direct comparisons to Java.

Vars and Vals

A big part of Kotlin is its changes to fields, getters, setters, and how they interact with parameters from constructors.

The following two snippets are effectively equivalent:

class VarsAndVals {
	var var1 = 0
	private var var2 = "variable string"

	val val1 = 0
	private val val2 = "value string"

	fun getting() {
		val local1 = var1
		val local2 = var2

		val local3 = val1
		val local4 = val2
	}

	fun setting() {
		var1 = 100
		var2 = "new value"
	}
}

public class VarsAndVals {
	private int var1 = 0;
	private String var2 = "variable string";
	
	private final int val1 = 0;
	private final String val2 = "value string";
	
	public int getVar1() {
		return var1;
	}
	
	public void setVar1(int var1) {
		this.var1 = var1;
	}
	
	private String getVar2() {
		return var2;
	}
	
	private void setVar2(String string) {
		this.var2 = string;
	}
	
	public int getVal1() {
		return val1;
	}
	
	private String getVar2() {
		return var2;
	}
	
	public void getting() {
		final int local1 = getVar1();
		final String local2 = getVar2();
		
		final int local3 = getVal1();
		final String local4 = getVal2();
	}
	
	public void setting() {
		setVar1(100);
		setVar2("new value");
	}
}

It's pretty clear that Kotlin saves a lot of work on the front of writing getters and setters. While this isn't too big of a deal, Kotlin makes itself invaluable in enforcing the usage of these functions in a syntactically shorter manner.

class VarsAndVals {
	var var1 = 0
	private var var2 = "variable string"

	val val1 = 0
	private val val2 = "value string"

	fun getting() {
		val local1 = var1
		val local2 = var2

		val local3 = val1
		val local4 = val2
	}

	fun setting() {
		var1 = 100
		var2 = "new value"
	}
}

public class VarsAndVals {
	private int var1 = 0;
	private String var2 = "variable string";
	
	private final int val1 = 0;
	private final String val2 = "value string";
	
	public int getVar1() {
		return var1;
	}
	
	public void setVar1(int var1) {
		this.var1 = var1;
	}
	
	private String getVar2() {
		return var2;
	}
	
	private void setVar2(String string) {
		this.var2 = string;
	}
	
	public int getVal1() {
		return val1;
	}
	
	private String getVar2() {
		return var2;
	}
	
	public void getting() {
		final int local1 = getVar1();
		final String local2 = getVar2();
		
		final int local3 = getVal1();
		final String local4 = getVal2();
	}
	
	public void setting() {
		setVar1(100);
		setVar2("new value");
	}
}

Kotlin enforces the use of getters and setters for property access, but uses the property access syntax!

Already, our Kotlin code is ~2.5 times shorter than Java.

If you're worried about defining custom getters and setters, Kotlin allows that too. More detail is available in the Kotlin docs. Kotlin allows for a fairly wide range of cool features around this concept.

Storing Constructor Parameters

Kotlin makes it super easy to take in constructor parameters and store them in the class. The following two snippets are also equivalent:

class ConstructorParams (val val1: String, var var1: Int)

public class ConstructorParams {
	private final String val1;
	private int var1;
	
	public ConstructorParams(String val1, int var1) {
		this.val1 = val1;
		this.var1 = var1;
	}
	
	public int getVar1() {
		return var1;
	}
	
	public String getVal1() {
		return val1;
	}
	
	public void setVar1(int var1) {
		this.var1 = var1;
	}
}

In this case, what was numerous lines in Java is only one in Kotlin. The Kotlin version is even a little easier to read!

Default Values In Methods and Constructors

Kotlin makes it easy to specify default values to functions and constructors. The following two snippets are equivalent:

class DefaultValues {
	fun function(arg: Int = 0) {
		val a = arg + 10
	}
}

public class DefaultValues {
	public void function(int arg) {
		int a = arg + 10;
	}
	
	public void function() {
		function(0);
	}
}

Kotlin makes this a little more powerful than demonstrated here, but for most situations, this is pretty straightforward.

Null Safety

Kotlin makes it easy to work with values that can and can't be null, much easier than Java:

val neverNull: Int = 10
val nullable: Int? = null

The ? lets us know that the variable could be null, and Kotlin will throw compilation warnings if we try to use it without checking and handling it.

Some more examples:

fun addOrThrow(a: Int?, b: Int?) {
    val safeA = a ?: throw IllegalStateException("a is null")
    val safeB = b!!
    return safeA + safeB
}

The ?: operator is known as the Elvis operator (after it's resemblance to Elvis Presley) and means that the code after it gets run only if the left-hand side is null. This allows the function to immediately exit and throw an error before any further processing occurs.

The !! operator is a shortcut for this operation. The Elvis operator is more powerful and flexible, but if you don't want to throw a specific error and instead crash immediately, the !! operator will enforce that the value isn't null.

Finally:

fun nullSafeMethodCall(a: Int?): Double {
    return a?.toDouble() ?: throw IllegalStateException("a is null")
}

We combine the concept above with the ?. operator, which performs a null safe method call. If a is null, Kotlin won't try to call .toDouble() on it and will just return null, which will then be caught by the Elvis operator!

Accessing Hardware

Because of Kotlin's null safety system, accessing hardware must be done differently. There are a few ways to do this, but the one we'll show here is to use by lazy:

    // by lazy will only initialize this variable the first time it is used.
    // This prevents it from ever being null, but also allows you to initialize it only after your opmode begins.
    val arm by lazy { hardwareMap["arm"] as DcMotorEx } // Alternately hardwareMap.get(DcMotorEx::class.java, "arm") also works here

    override fun init() { // or runOpMode() for LinearOpModes
        // Since we used by lazy, accessing the arm in any way will automatically initialize it:
        arm.power = 1.0
        telemetry.addData("armPos", arm.currentPosition)
        telemetry.update()
        // Note that, since we used by lazy, we do NOT need to put !! after arm.
    }

There are other options to do this as well such as the lateinit keyword.

Overview

Kotlin has a lot more to it than this short overview, but these are some of the features that make a big common difference with Java. Hopefully, this arms you with an expectation of what the rest of Kotlin is like, and some of the reasons that more advanced FTC programmers like to choose it over Java.

Kotlin official documentation

Overall, the best way to learn is just to jump in and give it a try. If you get stuck, search it up, or take a look at the docs again!

If you feel like you need a complete example of using Kotlin for FTC, I advise you ask in the FTC Discord. Most people who use Kotlin write fairly complex codebases and use different combinations of libraries, so you might need to ask some questions to find an example relevant to you.

Another good place to search for Kotlin code in an FTC context is in libraries. Many FTC software libraries such as Roadrunner are written in Kotlin, so they can provide great usage examples.

Last updated: 2024-05-29

Acronyms

Hardware and Electronics

CHub: Control Hub
EHub/ExHub: Expansion Hub
DS: Driver Station
RC: Robot Controller
ESD: Electrostatic Discharge
DC: Disconnect

SDK Built-In

SDK: Standard Development Kit
RTP: RUN_TO_POSITION Motor Mode
RUE: RUN_USING_ENCODERS Motor Mode
IMU: Internal Measurement Unit (Gyroscope)
EOCV/OCV: Easy OpenCV/OpenCV | Vision Processor, Pipelines
TFOD: TensorFlow Object Detection
hmap/hwmap: HardwareMap

Libraries

RR: RoadRunner (Motion Planning Library) 0.5.6, 1.0

Controllers+

Odo: Odometry
MP: Motion Profiling
FF: FeedForward
PID(F): Proportional Integral Derivative FeedForward Controller
KF/EKF: Kalman Filter / Extended Kalman Filter

Other

GH: GitHub
AS: Android Studio
OBJ: OnBot Java
ADB: Android Debug Bridge (used to wireless connect to control hub)

Last Updated: 2024-01-26

Improving Loop Times

Recipe

This recipe will cover various methods to improve loop times. At the end there are full examples of code that combine these methods

Why are fast loop times important?

The more often your teleop is updated, the more responsive it will be to button clicks and joystick changes. This can make driving easier. Additionally, if you are using PID(F) controllers, the more frequently they update the more accurate they are and the less they oscillate.

What causes slow loop times?

Surprisingly, the main cause of slow loop times is not processing difficulties or code complexity. Most of the processing time is spent on communicating with hardware devices, known as hardware "reads" and hardware "writes".

Hardware reads are when you are receiving data. For example, getting an encoder position, reading a color sensor, or accessing the IMU are all hardware reads. On the other hand, hardware writes are when you are sending data. For example, setting a motor power, setting a servo position, or configuring an LED are all hardware writes.

Checking loop times

Here is some basic code to measure your loop times in milliseconds. The more milliseconds your loop takes, the slower your loop times are.

This measures looptimes over a single loop, which can lead to unstable readings, but is good enough.

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;

public class MeasuringLoopTimes extends OpMode {
	private ElapsedTime elapsedtime;

	@Override
	public void init() {
		elapsedtime = new ElapsedTime();
		elapsedtime.reset();
	}

	@Override
	public void loop() {

		telemetry.addData("Loop Times", elapsedtime.milliseconds());
		elapsedtime.reset();
	}
}

You may want to try:

• Average looptimes over the whole runtime (totalRuntime / numberOfLoops).
• Measuring looptimes over a window (over 100 loops, over 50 loops, etc).

In order to get a better idea of your stable loop times.

Some of the optimisations we look at may increase the looptime variance, as some loops may be very short, while others may be much longer.

Bulk Reading

Other than I2C devices, reading can be done all at once in a "bulk read" for a huge loop time improvement.

By default, every time you do a hardware read, a new command is sent to retrieve it. Using one command to retrieve ALL the data is bulk reading.

This recipe will not go into the different bulk reading modes. To learn more look here.

Caching Motor Powers

So now let's try and reduce unnecessary hardware writes.

If a motor is going at 0.5 power, and we keep setting the power to 0.5, the output of the motor will not change. However, each one of those setPower() commands is a hardware write which will delay your loop. A simple solution to this is to only send a new motor power when it is different from the previous. The SDK has this built in for this 'exactly equal' case.

However, we can go even further then just difference to remove much more unnecessary writes. If the motor is currently running at 0.5 power, and you tell it to run at 0.51 power instead, it will have very little effect. However, it will unnecessarily perform a loop-delaying hardware write.

Instead, you can store the last power sent to a motor and check every new setPower() command to only run if the new power is sufficiently different from the previous power.

Implementations of hardware devices with these optimisations applied are available here. This github page provides the installation and usage instructions, so this recipe won't cover them.

Photon

Photon is another way of increasing loop times. Photon is an experimental library developed by Eeshwar, an alumni originally from team 7244. It allows for significantly faster loop times by parallelizing much more of the hardware reads and writes. Installation instructions for Photon are available here.

If you have Photon installed, you don't need to use CachingHardware, as Photon has its own caching hardware. Note that Photon is more advanced than CachingHardware, so it does the optimisations automatically.

Photon has a few known issues at the moment, here's some troubleshooting steps:

Some people have installed photon and Android Studio does not recognize the @Photon annotation. Instead of implementation 'com.github.Eeshwar-Krishnan:PhotonFTC:v3.0.1-ALPHA', try implementation 'com.github.Eeshwar-Krishnan:PhotonFTC:main-SNAPSHOT'.

Also, be warned. Photon sometimes when used just randomly reverses motors and servos (but it's always the same ones reversed the same way).

Full Examples

This example uses CachingHardware

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.util.ElapsedTime;

import java.util.List;

import dev.frozenmilk.dairy.cachinghardware.CachingDcMotorEx;

public class CachingOptimizedOpMode extends OpMode {

	private CachingDcMotorEx exampleMotor;
	private List<LynxModule> allHubs;
	private ElapsedTime elapsedtime;

	@Override
	public void init() {

		elapsedtime = new ElapsedTime();

		// this just sets the bulk reading mode for each hub
		allHubs = hardwareMap.getAll(LynxModule.class);
		for (LynxModule hub : allHubs) {
			hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
		}

		exampleMotor = new CachingDcMotorEx(hardwareMap.get(DcMotorEx.class, "example motor"));
		elapsedtime.reset();
	}

	@Override
	public void loop() {
		// clears the cache on each hub
		for (LynxModule hub : allHubs) {
			hub.clearBulkCache();
		}

		// after the first time, it won't actually send new commands
		exampleMotor.setPower(1);

		telemetry.addData("Motor Position", exampleMotor.getCurrentPosition());
		telemetry.addData("Loop Times", elapsedtime.milliseconds());
		elapsedtime.reset();
	}
}

This example uses Photon

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.util.ElapsedTime;

import java.util.List;

import com.outoftheboxrobotics.photoncore.Photon;

// note the annotation at the top of the op mode (this is all you have to do to use photon)
@Photon
public class PhotonOptimizedOpMode extends OpMode {

	private DcMotorEx exampleMotor;
	private List<LynxModule> allHubs;
	private ElapsedTime elapsedtime;

	@Override
	public void init() {

		elapsedtime = new ElapsedTime();

		// this just sets the bulk reading mode for each hub
		allHubs = hardwareMap.getAll(LynxModule.class);
		for (LynxModule hub : allHubs) {
			hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
		}

		exampleMotor = hardwareMap.get(DcMotorEx.class, "example motor");
		elapsedtime.reset();
	}

	@Override
	public void loop() {
		// clears the cache on each hub
		for (LynxModule hub : allHubs) {
			hub.clearBulkCache();
		}

		// after the first time, it won't actually send new commands
		exampleMotor.setPower(1);

		telemetry.addData("Motor Position", exampleMotor.getCurrentPosition());
		telemetry.addData("Loop Times", elapsedtime.milliseconds());
		elapsedtime.reset();
	}
}

Last Updated: 2024-10-14

Pedro Pathing vs Road Runner

Pedro Pathing is a recently-created library for autonomous.
It uses a GVF algorithm utilizing Bézier Curves to follow trajectories with speed as a top priority.

• Docs: https://pedropathing.com
• Quickstart: https://github.com/Pedro-Pathing/Quickstart

Pros of Pedro

• Can make your bot drive faster.
• Support for recent sensors (OTOS, Pinpoint) is official/built-in.
• Excellent correction for unexpected disturbances.

Cons of Pedro:

• Newer, so potentially less stable/buggier.
• Fewer people are familiar with it and able to help.
• Not necessarily time-consistent.
• Uses a nonstandard coordinate system by default/in visualizer.

Road Runner is a motion-profiling-based follower library that includes a command-based action system and geometry.

It was originally (0.5) created late 2020(?), with version 1.0 created mid-2023 and last updated 10/13.

It prioritizes time consistency above all else.

Library Repo: https://github.com/acmerobotics/road-runner/

Quickstart: https://github.com/acmerobotics/road-runner-quickstart/tree/master/

Official Docs: https://rr.brott.dev/docs/v1-0/installation/

Pros of Road Runner:

• Stable, minimal bugs if any
• Time consistent by default
• Extensively tested; used by thousands of teams
• Tons of people are able to help you in FTC Discord; someone has almost certainly had your problem before
• Lots of projects integrate with it
• Uses the FIRST-recommended standard coordinate system consistently

Cons of Roadrunner:

• Prioritizes time consistency above all else, meaning potentially worse correction
• Slower speed by default
• Support for recent sensors like the SparkFun OTOS and Pinpoint is unofficial (though still exists, made by j5155)

This page is also available on Pedro Docs. (Note: the Pedro team has updated the version on Pedro docs separately, and we cannot vouch for its accuracy.)

Last Updated: 2025-1-5

contributors

• hex (github) (discord)
• 0verkil (github) (discord)
• Ruckus Robotics (14712) (discord)
• JL (github) (discord)
• rjan939 (github) (discord)
• Froze 'n' Milk (github)
• Iris (github) (discord)
• j5155 (github) (discord)
• Arush (github) (discord)
• ZachWaffle (github) (discord)