# Welcome

SolversLib an updated and maintained fork of the older FTCLib. PRs made to both SolversLib & FTCLib are regularly merged into SolversLib. The changes made to SolversLib were designed to not break existing FTCLib code (with the exception of FTCLib's [vision](broken://pages/rZtsaB1AYEc7Mzde5fFA) module), allowing you to build upon preexisting FTCLib codebases.

FTCLib was initially meant to be a port of WPILib, which is the standard programming library for FRC that almost all teams use. However, with FTC, there are a ton of libraries that not many people have heard about, especially rookie teams who are just starting. The goal of FTCLib is to improve the initial programming experience for new members as well as greatly enhance the efficiency of code for veterans.

We support the transition of teams from programming systems like Blocks and OnBot Java to Android Studio. One of our goals is to make this transition easier for you if you have not already.

Please read the [installation](/installation) instructions before getting started with anything!

### SolversLib Leads:

* Arush Y (Owner) - [FTC 23511](https://www.seattlesolvers.com/)
* Saket T - [FTC 23511](https://www.seattlesolvers.com/)

### SolversLib Contributors:

* Oscar C - [Dairy Foundation](https://github.com/Dairy-Foundation)
  * Special thanks to Oscar for hosting SolversLib on the Dairy Foundation!
* Noam - [FTC 23644](https://www.youtube.com/@SkeletonArmy23644)
* Daniel - FTC 7854
* Nv7 - [FTC 21229](https://www.stem-bridge.org/home)
* Lucas B - FTC 15215, 22407, and 24736
* Suchir R - FTC [19448](https://www.instagram.com/xbots_ftc/)
* Kully - [KoalaLog](https://github.com/ori-coval)

### Docs Contributors:

* Arush Y (Owner) - [FTC 23511](https://www.seattlesolvers.com/)
* Dohun - [FTC 22489](https://www.youtube.com/@GNCEDiamond22489)
* Rowan M - [FTC 3805](https://atomic.ehrobotics.org/pages/team/)

### Contact:

* [SolversLib Discord](https://discord.com/invite/dJbSWcXsUp)
* [Email](mailto:software@seattlesolvers.com)
* [GitHub](https://github.com/FTC-23511/SolversLib)


# Changelog

This is the Changelog for SolversLib versions for 0.3.1 and higher. It includes the changes made from the previous iteration, and important notes for it as well. You can change SolversLib documentation versions by using the selector in the top left corner.

## [0.3.5](/)

#### Core:

* Added `preRun()` method to `CommandOpMode`, which runs once after "Play" is pressed and before the main `run()` loop starts
  * `preRun()` is not called if the OpMode is stopped during the init phase
* Bug fixed `CascadeController` velocity measurement to use the change in measured position instead of the change in error
* Bug fixed `CascadeController` outputting `NaN` on the first control loop cycle
* Bug fixed `CascadeController`'s primary controller not receiving the position setpoint
* Added `reset()` override to `CascadeController` that also resets its primary and secondary controllers
* Added `getMeasuredVel()` method to `CascadeController`

#### General:

* Changed the FTC SDK dependencies of all modules (`core`, `pedroPathing`, and `photon`) to `compileOnly`, so SolversLib no longer pins an SDK version and works with newer FTC SDK versions
* Split the license into [FIRST-LICENSE](https://github.com/FTC-23511/SolversLib/blob/master/FIRST-LICENSE) and [23511-LICENSE](https://github.com/FTC-23511/SolversLib/blob/master/23511-LICENSE)

## [0.3.4](https://docs.seattlesolvers.com/0.3.4)

#### Core:

* Fixed bug with `RunMode` in `MotorEx` using distance instead of position
* Added `ServoExGroup`
* Added optional `GamepadEx` Slew Rate Limiters (mostly for swerve, but can be used for mecanum chassis as well)
* Added optional `P2PController` Slew Rate Limiters (for swerve)
* Added `mirror()` method to `Pose2d` for alliance swapping
* Updated swerve kinematics with maxSpeed
* Refactored `Timing` class
  * `Stopwatch`
  * `Timer`
  * `Rate` (Refresh Rate Timer)
* Added `rawPower` tracking and access in hardware classes
* Bug fixed `MotorEx` 's PositionControl using `encoder.getPosition()` instead of `encoder.getDistance()`
* Added `LambdaCommand`
* Added `SubsystemBase` `isAvailable()` method
* Added CommandScheduler `getScheduledCommands()` method
* Added `Commands` utility class for pre-defined actions (not to be confused with the Command interface)
* Added `CallbackCommand`
* Bug fixed deadline `end()` command in `ParallelDeadlineGroup`

#### PedroPathing:

* Fixed maxPower not saving for multiple paths

#### Photon (NEW):

* Added stable PhotonCore for FTC

## [0.3.3](https://docs.seattlesolvers.com/0.3.3)

#### Core:

* Rewrote Hardware classes:
  * Deprecated [`ServoEx` interface](/features/hardware/motors-1#servoex-interface-and-simpleservo-class) and [`SimpleServo` class](/features/hardware/motors-1#servoex-interface-and-simpleservo-class)
    * The new [`ServoEx` class](/features/hardware/servos#servoex) serves as a replacement to both of the classes above.
  * Added [`CRServoEx`](/features/hardware/servos#crservoex-and-absoluteanalogencoder) and [`AnalogAbsoluteEncoder`](/features/hardware/servos#crservoex-and-absoluteanalogencoder) (4th wire support on Servos)
  * Added optional power caching to `Ex` classes like `MotorEx`, `ServoEx`, and `CRServoEx`, etc.
  * Added a Color Sensor class for the [`REV Color Sensor V3`](/features/hardware/sensors#sensors)
  * Modified [`SensorDistance`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistance.java) & [`SensorDistanceEx`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistanceEx.java) to handle custom minimum and maximum distance threshold values
* Beta Swerve support:
  * Added [`Point-to-Point`](/pathing/point-to-point) control
  * Rewrote Swerve kinematics and updated `ChassisSpeeds`
  * Created [`SolversLib Visualizer`](https://visualizer.seattlesolvers.com/) (forked from Pedro Pathing Visualizer)
* Implemented abstract [`Controller`](/features/controllers#using-the-controller-class) class and added [`SquIDF`](/features/controllers/squidf) controller
* Added native [`RetryCommand`](/command-base/command-system/convenience-commands#retrycommand) from Marrow

#### PedroPathing:

* Added two new Pedro Commands: [`TurnToCommand`](/pedro-pathing/pedro-commands/hold-point-command-2) and [`TurnCommand`](/pedro-pathing/pedro-commands/hold-point-command-1)
* Added support for Pedro Pathing versions 2.0.0 and higher

## [0.3.2](https://docs.seattlesolvers.com/0.3.2)

#### Core:

* Added beta SquID support and early Swerve Kinematics
* Fixed `CommandScheduler`'s `cancelAll()` from throwing an error
* Added additional constructors to [`RepeatCommand`](/command-base/command-system/convenience-commands#repeatcommand)
* Rewrote SolversHardware wrappers into hardware

#### PedroPathing:

* Added new Pedro Command: [`HoldPointCommand`](/pedro-pathing/pedro-commands/hold-point-command)
* Added support for Pedro Pathing 1.0.9

## [0.3.1](https://docs.seattlesolvers.com/0.3.1)

#### Core:

* Add SolversHardware caching wrappers
* Fixed known FTCLib bugs/issues:
  * [Double ending issue in ParallelRaceGroup](https://github.com/FTCLib/FTCLib/pull/261)
  * [SequentialCommandGroup is incompatible with other command groups](https://github.com/FTCLib/FTCLib/issues/253)
  * [ConcurrentModificationException in cancelAll()](https://github.com/FTCLib/FTCLib/pull/260)
  * [IllegalAccessError when trying to build clean dev branch on Windows](https://github.com/FTCLib/FTCLib/issues/255)
  * [Missing Aliases for PS4 and PS5 buttons](https://github.com/FTCLib/FTCLib/issues/250)

#### Pedro Pathing:

* Added new Pedro Command: [`FollowPathCommand`](/pedro-pathing/pedro-commands/followpathcommand)
* Added support for Pedro Pathing 1.0.8


# Installation

How to import SolversLib into your Android Studio FTC Project

## Option 1: Manually Install

This works with or without having [FTCLib](https://ftclib.org/) already installed. SolversLib allows you to easily migrate from FTCLib to SolversLib while keeping all of your code, or install normally.

### build.gradle

The first thing you need to change from FTCLib is the dependency in `build.gradle`

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
dependencies {
    // implementation "org.ftclib.ftclib:core:2.1.1" remove FTCLib core
    implementation "org.solverslib:core:0.3.5" // core
```

{% endcode %}

Or, if you are using [Pedro Pathing](/pedro-pathing/pedro-commands), change to this dependency block

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
dependencies {
    // implementation "org.ftclib.ftclib:core:2.1.1" remove FTCLib core
    implementation "org.solverslib:core:0.3.5" // core
    implementation "org.solverslib:pedroPathing:0.3.5" // pedroPathing
}
```

{% endcode %}

And if you are using [Photon](/photon/what-is-photon), change to this dependency block

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
dependencies {
    // implementation "org.ftclib.ftclib:core:2.1.1" remove FTCLib core
    implementation "org.solverslib:core:0.3.5" // core
    implementation "org.solverslib:pedroPathing:0.3.5" // pedroPathing
    implementation "org.solverslib:photon:0.3.5" // photon
}
```

{% endcode %}

{% hint style="info" %}
**New in 0.3.5:** SolversLib no longer pins a specific FTC SDK version (the SDK is now a `compileOnly` dependency), so it works with newer FTC SDK versions out of the box.
{% endhint %}

{% hint style="danger" %}
**Important:** Different versions of SolversLib correlate with different versions of Pedro Pathing.

[SolversLib 0.3.3](https://docs.seattlesolvers.com/0.3.3/installation)+ is for Pedro Pathing 2.0.0 and higher\
[SolversLib 0.3.2](https://docs.seattlesolvers.com/0.3.2/installation) is for Pedro Pathing 1.0.9\
[SolversLib 0.3.1](https://docs.seattlesolvers.com/0.3.1/installation) is for Pedro Pathing 1.0.8
{% endhint %}

The latest version numbers (as well as a list of all version numbers) are available at:

* Latest `core` version: <https://repo.dairy.foundation/#/releases/org/solverslib/core>
* Latest `pedroPathing` version: <https://repo.dairy.foundation/#/releases/org/solverslib/pedroPathing>
* Latest `photon` version: <https://repo.dairy.foundation/#/releases/org/solverslib/photon>

{% hint style="danger" %}
**Warning:** If you choose to use the Pedro Pathing module, you still need to [install Pedro Pathing 2.0.0](https://pedropathing.com/)+ in order to use it.
{% endhint %}

Please note that you should not and cannot have both FTCLib and SolversLib installed at the same time.

### Repositories (required)

Finally in your repositories block, add the following code. You may have other content here, especially if you have the Pedro Pathing library installed. If you do not have a repositories block, you can add it above your dependencies block.

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
repositories {
    maven {
        url "https://repo.dairy.foundation/releases"
    }
}
```

{% endcode %}

### Changing Imports (Only if Migrating from FTCLib)

Because the package names will be different, you can either manually replace all instances of `com.arcrobotics.ftclib` with `com.seattlesolvers.solverslib` , or use a command in a terminal to replace them all at once for you. Please make sure you either open a terminal into your Android Studio project or use the built-in Android Studio terminal to run the commands below.

**FTCLib Imports to SolversLib Imports (MacOS/Linux):**

```bash
find . -type f -name "*.java" -exec sed -i '' 's/com.arcrobotics.ftclib/com.seattlesolvers.solverslib/g' {} +
```

**SolversLib Imports to FTCLib Imports (MacOS/Linux):**

```bash
find . -type f -name "*.java" -exec sed -i '' 's/com.seattlesolvers.solverslib/com.arcrobotics.ftclib/g' {} +
```

**FTCLib Imports to SolversLib Imports (Windows):**

```powershell
Get-ChildItem -Recurse -Filter *.java | ForEach-Object { 
    (Get-Content $_.FullName) -replace 'com.arcrobotics.ftclib', 'com.seattlesolvers.solverslib' | 
    Set-Content $_.FullName 
}
```

**SolversLib Imports to FTCLib Imports (Windows):**

```powershell
Get-ChildItem -Recurse -Filter *.java | ForEach-Object { 
    (Get-Content $_.FullName) -replace 'com.seattlesolvers.solverslib', 'com.arcrobotics.ftclib' | 
    Set-Content $_.FullName 
}
```

### Sync Gradle and Finished!

![Click that button and if successful, you can now use SolversLib](https://content.gitbook.com/content/53OMM4PwbbzcZqInoTsn/blobs/7XZilhnnxtPA4vIrcTOG/gradle-sync.png)

## Option 2: Installing from SolversLib Quickstart

An alternative option is to simply use the SolversLib Quickstart. Similar to the FTCLib Quickstart, SolversLib has a Quickstart with this library fully set up. You can view it at <https://github.com/FTC-23511/SolversLib-Quickstart>. You can either fork or clone this repository as needed to use it.

In addition, the Quickstart also has the Pedro Pathing library installed and added along with the SolversLib `pedroPathing`\` dependency, meaning that it is hassle-free. If you don't want the Pedro Pathing part, you can simply delete the relevant files and dependencies.


# Javadocs

Javadocs are automatically created documentation for Java classes. It gives a basic decription on how to use those methods and classes, although a more in-depth explanation can be found within this documentation GitBook itself.

Below are the Javadocs for each module:

* `core`: <https://repo.dairy.foundation/javadoc/releases/org/solverslib/core/latest>&#x20;
* `pedroPathing`: <https://repo.dairy.foundation/javadoc/releases/org/solverslib/pedroPathing/latest>
* `photon`: <https://repo.dairy.foundation/javadoc/releases/org/solverslib/pedroPathing/latest>

You can replace `latest` with your desired version number to get Javadocs for that version.

{% hint style="info" %}
There are also auto Javadocs for beta versions/snapshots in addition to&#x20;
{% endhint %}


# Hardware

package com.seattlesolvers.solverslib.hardware

Each hardware device in SolversLib is based on the `HardwareDevice` interface. This comes with two methods inherited by every device:

* `disable()`: disables the device
* `getDeviceType()`: returns a String characterization of the device

Optional power caching has also been added to the `Ex` type classes as well to help further loop times as well.

SolversLib offers *a lot* of hardware devices that can be implemented or customized into your program. The best advice we can give to users is to take a look at the subpages in this catogery ([Servos](/features/hardware/servos), [Motors](/features/hardware/motors), and [Sensors](/features/hardware/sensors)) and the [hardware package](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/hardware) in the SolversLib repository.


# Servos

packages com.seattlesolvers.solverslib.hardware.servos and .motors

## ServoEx

The [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/servos/ServoEx.java) class allows for more methods and actions than the normal servo class in the SDK. You can change the position of the servo relative to the last position or set it to an absolute position. You can either specify a position within the range of the servo's motion or have it rotate a certain number of specified angle units.

It serves as a successor to the [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) interface and [SimpleServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SimpleServo.java) class, which have now been Deprecated. A copy of the original ServoEx interface documentation can be viewed [here](/features/hardware/motors-1#servoex-interface-and-simpleservo-class).

### Constructors:

#### 1. Regular (no Angle Control)

```java
ServoEx(HardwareMap hwMap, String id)

ServoEx servoEx = new ServoEx(hardwareMap, "servoEx");
```

#### 2. Angle Control (with Min and Max angle)

```java
ServoEx(HardwareMap hwMap, String id, double min, double max)
```

`MIN_ANGLE` and `MAX_ANGLE` are the minimum and maximum angle positions (in whatever angle unit you use consistently) you would like to set the servo. This functionally serves as the servo's effective range. The effective range is fixed at construction; to use a different range, construct a new `ServoEx` with the desired min and max.

#### 3. Angle Control (with Range)

```java
ServoEx(HardwareMap hwMap, String id, double range)
```

This is similar to the 2nd constructor, except that you specify only the angular range (from when the servo is set to 0 to 1); the minimum is assumed to be 0. The range is in whatever angle unit you choose to use consistently.

### Utility Methods:

You can use `setInverted()` invert the servo's direction as well:

```java
// invert the servo
servo.setInverted(true);

// get if the servo is inverted (true if inverted, false if not)
boolean isInverted = servo.getInverted();
```

To turn to positions and angles, utilize the following methods:

* `set`: sets the position (or angle, if a range or min + max were defined in the constructor) of the servo
* `get`: returns the position/angle of the servo based on the last write
* `getRawPosition`: returns the raw servo position from 0 to 1

### Power Caching

Additionally, as an `Ex` class, `ServoEx` supports caching. If the power set to that hardware is less than an adjustable threshold, it will not send a write to it to help with loop speeds. The default threshold, which is called `cachingTolerance` , is 0.0001.

You can use `.setCachingTolerance` to adjust `cachingTolerance` it as needed.

```java
ServoEx servoEx = new ServoEx(hardwareMap, "servoEx");

servoEx.setCachingTolerance(0.0001);
```

{% hint style="warning" %}
This default value of `0.0001` effectively disables the feature. To actually see results with power caching, increase it to `0.01` or higher depending on the precision required.
{% endhint %}

## ServoExGroup

The [ServoExGroup](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/servos/ServoExGroup.java) is like a CRServoGroup, but for `ServoEx`. A ServoExGroup object takes several ServoEx's and runs them in parallel like a single ServoEx. ServoEx groups have one leader and a set of followers. For any group, there *must* be a leader, but the number of followers can be zero. The constructor for a `ServoExGroup` is as follows:

```java
ServoExGroup myServos = new ServoExGroup(leader, follower1, follower2, ...);
```

The number of followers is variable. When you call `set()`, the leader is set first and every follower is then set to the leader's position, so you can very simply treat a `ServoExGroup` object like a single `ServoEx` object. The same is effectively true for other methods

{% hint style="info" %}
`setInverted()` on the group inverts *every* servo in it. If two servos are mounted mirrored, invert one of them individually before or inside the group.
{% endhint %}

```java
ServoEx left = new ServoEx(hardwareMap, "left");
ServoEx right = new ServoEx(hardwareMap, "right");

right.setInverted(true);

ServoExGroup servos = new ServoExGroup(left, right);

servos.set(0.6);
```

## CRServo

The [CRServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServo.java) class is just a motor object intended to be used for a continuous rotation servo. Its general purpose is to be used in SolversLib classes that require a `Motor` input. It works just like a regular motor, without any of the encoder stuff. As such, it extends the `Motor` class, and can be used in a [CRServoGroup](#crservogroup).

```java
CRServo crServo = new CRServo(hardwareMap, "CRServo");

crServo.set(0.5);
```

## CRServoEx & AbsoluteAnalogEncoder

The `CRServoEx` class is an advanced wrapper for continuous rotation servos (CRServos), adding key features for enhanced control and integration, including:

* **Absolute analog encoder support** (e.g., for Axon servos)
* **Optimized positional control using PIDF** (required for absolute encoder)
* **Power caching for improved loop performance**
* **Custom PWM range support**

It extends `CRServo`, which in turn extends the `Motor` class, and can also be used in a [CRServoGroup](#crservogroup).

The `AbsoluteAnalogEncoder` class is an advanced wrapper for Analog input AnalogInput absolute encoders, which are most commonly seen on servos with a 4th wire (like Axon Servos). It is best used in conjunction with `CRServoEx` .

### Constructors (AbsoluteAnalogEncoder)

#### 1. Basic AbsoluteAnalogEncoder

```java
AbsoluteAnalogEncoder(HardwareMap hwMap, String id)

// Example
AbsoluteAnalogEncoder encoder = new AbsoluteAnalogEncoder(hardwareMap, "absoluteEncoder");
```

This defaults to a range of 3.3, and an `AngleUnit` of Radians. If you do not know what you need, this is most likely it (and is functional for Axon servos).

#### 2. Advanced AbsoluteAnalogEncoder

```java
AbsoluteAnalogEncoder(HardwareMap hwMap, String id, double range, AngleUnit angleUnit)

// Example
AbsoluteAnalogEncoder absoluteEncoder = new AbsoluteAnalogEncoder(hwMap, "encoder", 3.3, AngleUnit.RADIANS)
```

This allows you to set your own range and `AngleUnit` if necessary. These constructors are used automatically in the designated CRServoEx constructors below as well.

* `angleUnit`: Angle unit for moving servo to position (`AngleUnit.DEGREES` or `AngleUnit.RADIANS`)
* `analogRange`: Voltage range for the encoder (e.g., 3.3V or 5V, depending on hardware)
  * **Default: 3.3V**
* `encoderID`: Name of the absolute encoder in hardware map

### Constructors (CRServoEx)

#### 1. Basic CRServo (Raw Power)

```java
CRServoEx(HardwareMap hwMap, String id)
```

* No encoder required; runs in `RawPower` mode by default. This is similar to `CRServo` , with a few extra features.

#### 2. Positional Control with Encoder

```java
CRServoEx(HardwareMap hwMap, String id, AbsoluteAnalogEncoder absoluteEncoder, RunMode runmode)

// Example
CRServoEx crServoEx = new CRServoEx(hardwareMap, "crServoEx", encoder, CRServoEx.RunMode.OptimizedPositionalControl)

```

* `hwMap`: FTC HardwareMap instance
* `id`: Configuration name of the CRServo
* `absoluteEncoder`: Instance of `AbsoluteAnalogEncoder` (must be initialized separately)
* `runmode`: Mode to run (see below)

{% hint style="danger" %}
**Warning:** If you have set the runmode to `OptimizedPositionalControl`, regardless if you want angle-based control, you must use a valid `AbsoluteAnalogEncoder` and also set the `PIDF` (see below for more information). Failing to do so will result in an error being thrown.
{% endhint %}

#### 3. Advanced Encoder Configuration

```java
CRServoEx(HardwareMap hwMap, String id, String encoderID, double analogRange, AngleUnit angleUnit, RunMode runmode)
```

Instead of passing an `AbsoluteAnalogEncoder` in, this overloaded constructor has parameters to create one inside of the instance of the `CRServoEx`.

* Allows specifying encoder parameters:
  * `encoderID`: Name of the absolute encoder in hardware map
  * `analogRange`: Voltage range for the encoder (e.g., 3.3V or 5V, depending on hardware)
    * **Default: 3.3V**
  * `angleUnit`: Angle unit for moving servo to position (`AngleUnit.DEGREES` or `AngleUnit.RADIANS`)
    * **Default**: If not specified, defaults to `AngleUnit.RADIANS` unless overridden
  * `runmode`: See below

### Using a CRServoEx RunMode

Like the `Motor`'s RunMode, the CRServoEx RunMode is a method of running the CRServoEx when power is supplied. However, there are only two modes: `OptimizedPositionalControl`, and `RawPower`.

```java
// in CRServoEx.java

/**
 * The mode in which the CR servo should behave.
 */
public enum RunMode {
    /**
     * Mode in which the CR servo takes the shortest path to reach a specific angle
     * Requires PIDF tuning (see below) + absolute encoder
     */
    OptimizedPositionalControl,
    /**
     * Default mode in which the CR servo is controlled with raw power
     */
    RawPower
}
```

#### `OptimizedPositionalControl`

* **Requirements**: An absolute encoder and PIDF coefficients.
* **Behavior**: When you call `.set(angle)`, the servo will move to the target angle using the shortest path.
  * Example:

    ```java

    AbsoluteAnalogEncoder encoder = new AbsoluteAnalogEncoder(hardwareMap, "absoluteEncoder");
    CRServoEx crServoEx = new CRServoEx(hardwareMap, "crServoEx", encoder, CRServoEx.RunMode.OptimizedPositionalControl)

    crServoEx.setPIDF(new PIDFCoefficients(0.001, 0.0, 0.1, 0.0001));
    crServoEx.set(Math.toRadians(90)); // move to 90 degrees (in radians)
    ```
  * If PIDF not set, positional control will throw an error.

#### `RawPower`

* Acts like a normal CRServo's `RawPower`.

### PIDF Control

For positional control, you **must** set PIDF coefficients:

```java
s_crServoEx.setPIDF(new PIDFCoefficients(double P, double I, double D, double F));
```

* These coefficients will be used by the internal PIDF controller to compute the necessary power to reach the target angle smoothly and quickly.

{% hint style="warning" %}
**Important:** These PIDF coeffecients are used in SolversLib's PIDF class for the calculations. As such, you should tune it as if you were to tune a normal PIDF.
{% endhint %}

### Power Caching

Finally, like `MotorEx` , `CRServoEx` supports power caching. If the power set to that hardware is less than an adjustable threshold, it will not send a write to it to help with loop speeds. The default threshold, which is called `cachingTolerance` , is 0.0001.

You can use `.setCachingTolerance` to adjust `cachingTolerance` it as needed.

```java
CRServoEx s_crServoEx = new CRServoEx(hardwareMap, "s_crServoEx");

s_crServoEx.setCachingTolerance(0.0001);
```

### Example: Full Setup

```java
AbsoluteAnalogEncoder encoder = new AbsoluteAnalogEncoder(hardwareMap, "absoluteEncoder");
CRServoEx crServoEx = new CRServoEx(hardwareMap, "s_crServoEx", encoder, CRServoEx.RunMode.OptimizedPositionalControl);
crServoEx.setPIDF(new PIDFCoefficients(0.8, 0.02, 0.1, 0.0));
crServoEx.setCachingTolerance(0.0002);

crServoEx.set(Math.toRadians(135));
```

### Additional Methods

* `.setRunMode(RunMode runmode)`: Change runmode after construction.
* `.setAbsoluteEncoder(AbsoluteAnalogEncoder encoder)`: Switch encoder instance.
* `.getAbsoluteEncoder()`: Get associated encoder.
* `.getCachingTolerance()`: Retrieve current tolerance.
* `.getController()`: Get extended controller instance.
* `.getServo()`: Get underlying SDK CRServo object.
* `.getDeviceType()`: Returns device type (string).
* `.setPwm(PwmControl.PwmRange pwmRange)`: Sets the PWM range for the servo using the SDK's `PwmControl.PwmRange`

For more details, refer to the Javadocs within `CRServoEx.java`. The class supports method chaining for convenient setup and configuration.

## CRServoGroup

The CRServoGroup is like a MotorGroup, but for CRServo/CRServoEx. A CRServo group object takes several CRServos and runs them in parallel like a single CRServo. CRServo groups have one leader and a set of followers. For any group, there *must* be a leader, but the number of followers can be zero. This makes creating different drive profiles simpler. The constructor for a `CRServoGroup` is as follows:

```java
CRServoGroup myCRServos = new CRServoGroup(leader, follower1, follower2, ...);
```

The number of followers is variable. The other methods of the `CRServoGroup` are the same as the ones found in `CRServo`. You can very simply treat a `CRServoGroup` object like a single `CRServo` object. The [flywheel sample](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/FlywheelSample.java) in the examples folder shows a few other methods you can utilize with the `MotorGroup`.

{% hint style="danger" %}
**Warning:** CRServo/CRServoEx should use a CRServoGroup (this) instead of a [MotorGroup](/features/hardware/motors#motorgroup)
{% endhint %}


# Motors

package com.seattlesolvers.solverslib.hardware.motors

SolversLib offers fully-featured motor wrappers for the ease of the user. Behind the scenes, it utilizes the advanced features of SolversLib to produce a more powerful implementation of the DcMotor objects offered in the SDK. Similarly, SolversLib has a `Motor` and `MotorEx` object, each of which allow for the user to directly access the instance object from the hardware map in the case of wanting to work with it directly.

## Creating a Motor Object

Creating a motor is as simple as passing in the hardware map, the name of the device in the robot controller config, and an optional third parameter of a GoBILDA motor type. This is necessary because the goBILDA motors in the configuration don't specify the different max RPM (rotations per minute) and CPR (counts per revolution).

There is also an option of using a custom CPR and RPM value.

```java
Motor m_motor_1 = new Motor(hardwareMap, "motorOne");
Motor m_motor_2 = new Motor(hardwareMap, "motorTwo", GoBILDA.RPM_312);
Motor m_motor_3 = new Motor(hardwareMap, "motorThree", CPR, RPM);

// grab the internal DcMotor object
DcMotor motorOne = m_motor_1.motor;
```

### Using a RunMode

A RunMode is a method of running the motor when power is supplied. There are three modes: `VelocityControl`, `PositionControl`, and `RawPower`.

```java
// in Motor.java

/**
 * The RunMode of the motor.
 */
public enum RunMode {
    VelocityControl, PositionControl, RawPower
}
```

Raw power sets the motor power directly through a value of $$\[-1, 1]$$ where the value represents the percentage of its maximum speed. This is also open loop control, which means there is no feedback control. This is the default mode of the motor.

```java
// set the run mode
m_motor.setRunMode(Motor.RunMode.RawPower);

// set the proportional output power of the motor
m_motor.set(0.37);    // 37% of maximum speed in current direction
```

Position control has the motor run to a desired position based on the input speed and the distance between current motor position and target position (in counts). This utilizes a P controller whose coefficient can be changed using `setPositionCoefficient(double)`. This is a tuned value. For tuning, we currently recommend using [FTC Dashboard](https://acmerobotics.github.io/ftc-dashboard/basics).

```java
// set the run mode
m_motor.setRunMode(Motor.RunMode.PositionControl);

// set and get the position coefficient
m_motor.setPositionCoefficient(0.05);
double kP = m_motor.getPositionCoefficient();

// set the target position
m_motor.setTargetPosition(1200);      // an integer representing
                                      // desired tick count

m_motor.set(0);

// set the tolerance
m_motor.setPositionTolerance(13.6);   // allowed maximum error

// perform the control loop
while (!m_motor.atTargetPosition()) {
  m_motor.set(0.75);
}
m_motor.stopMotor(); // stop the motor

/* ALTERNATIVE TARGET DISTANCE */

// configure a distance per pulse,
// which is the distance traveled in a single tick
// dpp = distance traveled in one rotation / CPR
m_motor.setDistancePerPulse(0.015);

// set the target
m_motor.setTargetDistance(18.0);

// this must be called in a control loop
m_motor.set(0.5); // mode must be PositionControl
```

Velocity control has the motor run using velocity in ticks per second with both a feedback and feedforward controller rather than simply setting the speed to a percentage of the maximum output speed. This can lead to smoother control for your motors, and is highly recommended for autonomous programs.

```java
// set the run mode
m_motor.setRunMode(Motor.RunMode.VelocityControl);

// set and get the coefficients
m_motor.setVeloCoefficients(0.05, 0.01, 0.31);
double[] coeffs = m_motor.getVeloCoefficients();
double kP = coeffs[0];
double kI = coeffs[1];
double kD = coeffs[2];

// set and get the feedforward coefficients
m_motor.setFeedforwardCoefficients(0.92, 0.47);
double[] ffCoeffs = m_motor.getFeedforwardCoefficients();
double kS = ffCoeffs[0];
double kV = ffCoeffs[1];

m_motor.setFeedforwardCoefficients(0.92, 0.47, 0.3);
ffCoeffs = m_motor.getFeedforwardCoefficients();
kA = ffCoeffs[2];

// set the velocity of the motor
// this must be called in a control loop
m_motor.set(-0.54); // 54% of maximum velocity, backwords
```

### Setting Behaviors

SolversLib, like the SDK, has wrapper methods for setting the ZeroPowerBehavior and the direction of the motors. ZeroPowerBehavior is often used for mechanisms other than the drivetrain; for example, like how you would use the BRAKE behavior for a lift.

```java
// set the inversion factor
m_motor.setInverted(true);

// get the inversion factor
boolean isInverted = m_motor.getInverted();

// set the zero power behavior to BRAKE
m_motor.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);

```

### The Built-In Encoder

A lot of motors have built-in encoders. SolversLib offers a nested class `Motor.Encoder` that utilizes advanced mechanics and corrects for [velocity overflow](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/241). One of the other great things is that resetting the encoder does not require stopping the motor. It calculates an offset and subtracts that from the current position. This offset is set to the current position of the encoder each time the `reset()` method is called. The Motor object also has methods that manipulate the encoder so that you don't have to grab the internal encoder instance from the object.

You can also use the built-in encoder as an encoder itself when using an external encoder.

```java
// reset the encoder
m_motor.resetEncoder();

// the current position of the motor
int pos = m_motor.getCurrentPosition();

// get the current velocity
double velocity = m_motor.getVelocity(); // only for MotorEx
double corrected = m_motor.getCorrectedVelocity();

// grab the encoder instance
Motor.Encoder encoder = m_motor.encoder;

// get number of revolutions
double revolutions = encoder.getRevolutions();

// set the distance per pulse to 18 mm / tick
encoder.setDistancePerPulse(18.0);
m_motor.setDistancePerPulse(18.0); // also an option

// get the distance traveled
double distance = encoder.getDistance();
distance = m_motor.getDistance(); // also an option

/** USEFUL FEATURES **/

// you can set the encoder of the motor to a different motor's
// encoder
m_motor.encoder = other_motor.encoder;

// sometimes the encoder needs to be reset completely
// through hardware
m_motor.stopAndResetEncoder();
```

## The MotorEx Object

`MotorEx` is an implementation of the Motor class with better integrated velocity control. Unlike the Motor object, it uses the corrected velocity by default instead of the raw velocity returned by the SDK's encoder estimates. It also uses the `DcMotorEx` object instead of the `DcMotor`. Calling `getVelocity()` will return the velocity.

You can also set the velocity directly using `setVelocity()`. You can pass the angular rate and the angle unit (optional). Passing just the angular rate will set the velocity in ticks per second. Passing an angle unit will set the velocity to units per second, depending on the unit that is passed into the method.

Additionally, `MotorEx` supports power caching. If the power set to that hardware is less than an adjustable threshold, it will not send a write to it to help with loop speeds. The default threshold, which is called `cachingTolerance` , is 0.0001.&#x20;

You can use `.setCachingTolerance` to adjust `cachingTolerance` it as needed.

```java
MotorEx m_motor = new MotorEx(hardwareMap, "motorEx");

m_motor.setCachingTolerance(0.0001);
```

{% hint style="warning" %}
This default value of `0.0001`  effectively disables the feature. To actually see results with power caching, increase it to `0.01` or higher depending on the precision required.
{% endhint %}

### Bulk Reading

A bulk read reads all of the sensor data (except I2C) on a lynx module to save cycle times. Bulk reads were introduced in SDK version 5.4. Since SolversLib uses wrappers, we can treat them the same way as other sensors.

Here's a sample implementation of auto-caching.

```java
// obtain a list of hubs
List<LynxModule> hubs = hardwareMap.getAll(LynxModule.class);

MotorEx m1 = new MotorEx(hardwareMap, "one");
MotorEx m2 = new MotorEx(hardwareMap, "two");

for (LynxModule hub : hubs) {
     hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}

// control loop
int cycles = 0;
while (cycles++ < 500) {
    double v1 = m1.getVelocity();
    double v2 = m2.getVelocity();

    /* implementation */
}
```

You can also take a look at [this sample](https://github.com/FIRST-Tech-Challenge/FtcRobotController/blob/master/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java) in the SDK.

## MotorGroup

A motor group object takes several motors and runs them in parallel like a single motor. Motor groups have one leader and a set of followers. For any group, there *must* be a leader, but the number of followers can be zero. This makes creating different drive profiles simpler. The constructor for a `MotorGroup` is as follows:

```java
MotorGroup myMotors = new MotorGroup(leader, follower1, follower2, ...);
```

The number of followers is variable. The other methods of the `MotorGroup` are the same as the ones found in `Motor`. You can very simply treat a `MotorGroup` object like a single `Motor` object. The [flywheel sample](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/FlywheelSample.java) in the examples folder shows a few other methods you can utilize with the `MotorGroup`.

{% hint style="danger" %}
**Warning:** CRServo/CRServoEx should use a CRServoGroup instead of a [MotorGroup](#motorgroup)
{% endhint %}


# Sensors

package com.seattlesolvers.solverslib.hardware

## Sensors

There are a few sensors that are offered in SolversLib:

* [SensorColor](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorColor.java) & [SensorRevColorV3](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorRevColorV3.java)
* [SensorDistance](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistance.java) & [SensorDistanceEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistanceEx.java)
* [SensorRevTOFDistance](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorRevTOFDistance.java)

The `SensorColor` class is just an extension for the `ColorSensor` class that is in the SDK, and the `SensorRevColorV3` is the same but for the [REV Color Sensor V3](https://www.revrobotics.com/rev-31-1557/).

`SensorDistance` and `SensorDistanceEx` are interfaces for creating custom distance sensors if desired. An implementation of the `SensorDistanceEx` interface is `SensorRevTOFDistance` which utilizes the time-of-flight mechanic to track distance. It has a minimum and maximum tolerance, that can be used via `DistanceTarget`.

## Gyro Extensions

The `GyroEx` class is an extended gyro that allows users to add more configurable methods and possible control to their gyro. An example would be creating a `ModernRoboticsGyro` class. The abstract class has the following methods:

* `init()`: initializes the gyro and sets the current direction to the 0 heading
* `getHeading()`: returns the heading of the robot compared to the last reset
* `getAbsoluteHeading()`: returns the absolute heading relative to the initial direction
* `getAngles()`: returns the x, y, and z orientation of the gyro. This is functionally the same as yaw, pitch, and roll.
* `getRotation2d()`: transforms the heading into a `Rotation2d` object
* `reset()`: applies an offset so that `getHeading()` returns the 0 position

A useful implementation of this is the [RevIMU](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/RevIMU.java) class for the built-in imu on your REV hub.


# Deprecated Hardware

Deprecated Classes & Interfaces

### SolversHardware

SolversLib used to offers much simpler motor wrappers for motor caching & easy usage of [Axon Servos](https://axon-robotics.com/collections/servos). However, it is now deprecated, as these features have been merged into SolversLib's [native hardware classes](/features/hardware), which has been rewritten to better support it.

### ServoEx Interface and SimpleServo Class

Additionally, the [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) interface and [SimpleServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SimpleServo.java) class have been Deprecated, in favor of the new and superior [ServoEx](https://github.com/FTC-23511/SolversLib/blob/public-beta/core/src/main/java/com/seattlesolvers/solverslib/hardware/servos/ServoEx.java) class (which has [updated documentation](/features/hardware/servos#servoex)). A copy of the original ServoEx interface documentation is below.

The [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) interface allows for more methods and actions than the normal servo class in the SDK. You can change the position of the servo relative to the last position or set it to an absolute position. You can either specify a position within the range of the servo's motion or have it rotate a certain number of specified angle units.

An example implementation of this can be found in the [SimpleServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SimpleServo.java) class. You can create a simple servo like this:

```java
ServoEx servo = new SimpleServo(
    hardwareMap, "servo_name", MIN_ANGLE, MAX_ANGLE
);

// the above is functionally equivalent to
servo = new SimpleServo(
    hardwareMap, "servo_name", MIN_ANGLE, MAX_ANGLE,
    AngleUnit.DEGREES
);

// if you want to set the range in radians in the constructor
// you can use the following
servo = new SimpleServo(
    hardwareMap, "servo_name", MIN_ANGLE, MAX_ANGLE,
    AngleUnit.RADIANS
);
```

`MIN_ANGLE` and `MAX_ANGLE` are the minimum and maximum angle positions in degrees you would like to set the servo. This functionally serves as the servo's effective range. If you want to change the effective range at any point, you can do the following:

```java
// change the effective range to a min and max in DEGREES
servo.setRange(MIN_ANGLE, MAX_ANGLE);

// change the range to a min and max in RADIANS
servo.setRange(MIN_ANGLE, MAX_ANGLE, AngleUnit.RADIANS);

// return the effective range
double degreeRange = servo.getAngleRange();

// return the effective range in RADIANS
degreeRange = servo.getAngleRange(AngleUnit.RADIANS);
```

You can invert the servo's direction as well:

```java
// invert the servo
servo.setInverted(true);

// get if the servo is inverted (true if inverted, false if not)
boolean isInverted = servo.getInverted();
```

To turn to positions and angles, utilize the following methods:

* `rotateByAngle`: turns the servo a number of angle units relative to the current angle
* `turnToAngle`: sets the absolute angle of the servo
* `rotateBy`: turns the servo a relative positional distance from the current position
* `setPosition`: set the absolute position of the servo (from 0 to 1)


# Gamepad

package com.seattlesolvers.solverslib.gamepad

SolversLib provides enhanced Gamepad features. These classes are essentially extensions of the stock FTC SDK Gamepad features but with easier implementation methods.

## GamepadKeys

Provides enum representations of the buttons, D-Pad, bumpers, and triggers. Buttons, D-Pad, and bumpers are stored in `GamepadKeys.Button` and triggers are stored in `GamepadKeys.Trigger`. SolversLib has updated support for PS4 / PS5 controllers as well.

| Buttons              |
| -------------------- |
| Y                    |
| X                    |
| A                    |
| B                    |
| TRIANGLE             |
| CIRCLE               |
| SQUARE               |
| CROSS                |
| LEFT\_BUMPER         |
| RIGHT\_BUMPER        |
| BACK                 |
| START                |
| DPAD\_UP             |
| DPAD\_DOWN           |
| DPAD\_LEFT           |
| DPAD\_RIGHT          |
| LEFT\_STICK\_BUTTON  |
| RIGHT\_STICK\_BUTTON |
| PS                   |
| SHARE                |
| TOUCHPAD             |
| TOUCHPAD\_FINGER\_1  |
| TOUCHPAD\_FINGER\_2  |

| Trigger        |
| -------------- |
| LEFT\_TRIGGER  |
| RIGHT\_TRIGGER |

```java
// these are from the GamepadButton class that is used
// for command-based frameworks
GamepadButton grabButton = new GamepadButton(
    gamepad1, GamepadKeys.Button.A
);
GamepadButton releaseButton = new GamepadButton(
    gamepad2, GamepadKeys.Button.B
);

GamepadEx gamepadEx = new GamepadEx(gamepad1);
```

## GamepadEx

An extension of the stock FTC SDK `Gamepad` class. Constructed simply from a Gamepad. Provides six intuitive value-getting methods:

* `getButton()`: Given a `GamepadKeys.Button`, this method will check if that Button is pressed, returning a boolean of whether that Button is pressed.

```java
gamepadEx.getButton(GamepadKeys.Button.A);
```

* `getTrigger()`: Given a `GamepadKeys.Trigger`, this method will return the value of the Trigger (0 if unpressed, 1 if fully depressed).

```java
gamepadEx.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
```

* `getLeftY()`: Returns the value of the y-axis of the left joystick (note that the value returned is the opposite of what would be returned from the standard gamepad object).

```java
gamepadEx.getLeftY();
```

* `getRightY()`: Returns the value of the y-axis of the right joystick

```java
gamepadEx.getRightY();
```

* `getLeftX()`: Returns the value of the x-axis of the left joystick

```java
gamepadEx.getLeftX();
```

* `getRightX()`: Returns the value of the x-axis of the right joystick

```java
gamepadEx.getRightX();
```

## KeyReader

The `KeyReader` interface is the base for objects that monitor an individual button or trigger on a gamepad. All `Reader` classes must implement these functions:

* `readValue()`: Reads the current value of the key, true or false, and updates the values used by the reader. Returns nothing. This must be called once every loop.
* `isDown()` : Checks if key is currently down. Will return a boolean of whether that key is pressed.
* `wasJustPressed()` : Returns boolean whether the key is pressed, but only if it was previously not pressed.
* `wasJustReleased()` : Returns boolean indicating whether the key is not pressed, but only if it was previously pressed.
* `stateJustChanged` : Returns boolean indicating that the key's value has switched.

## TriggerReader

The `TriggerReader` class implements the `KeyReader` interface. Because `GamepadEx` Triggers return a `double` , the `TriggerReader` class interprets a value of greater than `0.5` as a trigger press.

The following constructs a new Trigger Reader with a `GamepadEx` gamepad and `GamepadKeys.Trigger` trigger.

```java
TriggerReader triggerReader = new TriggerReader(
    gamepadEx, GamepadKeys.Trigger.RIGHT_TRIGGER
);
```

Below are the different methods you can use with the trigger reader.

```java
triggerReader.isDown();
triggerReader.readValue();
triggerReader.stateJustChanged();
triggerReader.wasJustPressed();
triggerReader.wasJustReleased();
```

## ButtonReader

The `ButtonReader`class implements the `KeyReader` interface. It checks if a button is pressed, released, or is down.

```java
ButtonReader reader = new ButtonReader(
    gamepadEx, GamepadKeys.Button.A
);
```

* `ButtonReader(GamepadEx gamepad, GamepadKeys.Button button)`: Constructs a new Button Reader with a `GamepadEx` gamepad and a `GamepadKeys.Button` button.
* `ButtonReader(BooleanSupplier supplier)`: Constructs a new Button Reader using the value of a boolean supplier instead of a gamepad, which allows reading value states easily without a gamepad.

```java
reader.readValue();
reader.wasJustPressed();
reader.stateJustChanged();
reader.isDown();
reader.wasJustReleased();
```

The `GamepadEx` objects actually contain `ButtonReader`s. For every `GamepadKeys.Button`, there is a matching `ButtonReader` entry in the map. It is stored internally as a `Map<GamepadKeys.Button, ButtonReader>`. This allows you to use these features just with the `GamepadEx` class.

```java
// create the gamepad
GamepadEx myGamepad = new GamepadEx(gamepad1);

/** The methods for using the ButtonReaders **/
myGamepad.wasJustPressed(GamepadKeys.Button.A);
myGamepad.stateJustChanged(GamepadKeys.Button.A);
myGamepad.isDown(GamepadKeys.Button.A);
myGamepad.wasJustReleased(GamepadKeys.Button.A);

// pass the GamepadKeys.Button that you want to read
// into the method argument

// to read all buttons at once, perform a single call
myGamepad.readButtons();
/*
this is the equivalent of calling readValue() once
for all your readers
*/
```

## ToggleButtonReader

```java
ToggleButtonReader toggleButtonReader = new ToggleButtonReader(
    gamepadEx, GamepadKeys.Button.A
);
```

The `ToggleButtonReader` class extends `ButtonReader` and adds the ability to get the status of a toggle. `readValue()` needs to be run in a loop to get the state of the toggle.

`getState()` : Gets the toggle value of a button or boolean supplier.

```java
toggleButtonReader.getState();
```

### Usage

```java
GamepadEx toolOp = new GamepadEx(gamepad2);
ToggleButtonReader aReader = new ToggleButtonReader(
  toolOp, GamepadKeys.Button.A
);

while (...) {
  if (aReader.getState()) {
    // if toggle state true
  } else {
    // if toggle state false
  }
  aReader.readValue();
}
```

## Slew Rate Limiters

The [SlewRateLimiter](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/gamepad/SlewRateLimiter.java) class caps how fast a joystick value is allowed to change. Instead of the output jumping instantly from 0 to 1 when you slam the stick, it ramps toward the target at a maximum rate you choose, smoothing out acceleration. This reduces wheel slip and tipping on drivetrains, and is especially popular for swerve drives, where sudden input changes can fight against module rotation.

Credit to FTC 16379 Kookybotz for this code.

### SlewRateLimiter API

Rate limits are in units per second. A joystick axis ranges from -1 to 1, so a rate limit of `2.0` lets the output go from 0 to full deflection in half a second.

* `SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue)`: Constructs a limiter with separate limits for increasing and decreasing output (the negative rate limit should be a negative number) and a starting output value.
* `SlewRateLimiter(double rateLimit, double initialValue)`: Constructs a limiter with a symmetric limit of ±`rateLimit` and a starting output value.
* `SlewRateLimiter(double rateLimit)`: Constructs a limiter with a symmetric limit of ±`rateLimit` and a starting output value of 0.
* `calculate(double input)`: Returns the input, limited so it changes no faster than the rate limits allow since the previous call. Call this once per loop with the raw value.
* `updateRateLimit(double rateLimit)`: Changes the rate limit on the fly, setting the positive limit to `rateLimit` and the negative limit to `-rateLimit`.

```java
SlewRateLimiter limiter = new SlewRateLimiter(4.0);

// in your loop
double smoothed = limiter.calculate(gamepad1.left_stick_x);
```

{% hint style="warning" %}
A `SlewRateLimiter` is stateful — it remembers its previous output and an internal timer. Construct it once (e.g. in `init`) and reuse it every loop. Creating a new limiter each loop resets that state, so the output stays pinned near the initial value and the stick will barely respond.
{% endhint %}

### Enabling through GamepadEx

`GamepadEx` can apply slew rate limiting for you. Attach a limiter to each joystick axis with `setJoystickSlewRateLimiters(SlewRateLimiter LX, SlewRateLimiter LY, SlewRateLimiter RX, SlewRateLimiter RY)`, where L/R is the left/right joystick and X/Y is the axis. Pass `null` for any axis you do not want limited. The method returns the `GamepadEx` object for chaining.

Once set, `getLeftX()`, `getLeftY()`, `getRightX()`, and `getRightY()` automatically return the rate-limited values — no extra calls needed in your loop. Note that limiting only applies through these `GamepadEx` getters, not when reading the stock `gamepad` fields directly.

```java
GamepadEx driverOp = new GamepadEx(gamepad1)
    .setJoystickSlewRateLimiters(
        new SlewRateLimiter(4.0), // left stick x
        new SlewRateLimiter(4.0), // left stick y
        new SlewRateLimiter(6.0), // right stick x (turning can respond faster)
        null                      // right stick y (not limited)
    );
```

### Usage

```java
GamepadEx driverOp = new GamepadEx(gamepad1)
    .setJoystickSlewRateLimiters(
        new SlewRateLimiter(4.0),
        new SlewRateLimiter(4.0),
        new SlewRateLimiter(6.0),
        null
    );

waitForStart();

while (opModeIsActive()) {
    double strafe = driverOp.getLeftX();
    double forward = driverOp.getLeftY();
    double turn = driverOp.getRightX();

    drive.driveRobotCentric(strafe, forward, turn);
}
```


# Drivebases

package com.seattlesolvers.solverslib.drivebase

All of the [SolversLib drivebase classes](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/drivebase) are based on the `RobotDrive` abstract class. This is functionally similar to the [RobotDriveBase ](https://github.com/wpilibsuite/allwpilib/blob/50db653f8d864c594c6a9ab7dd5a0f45b4483a03/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java)class in WPILib. It contains:

* An enumerator for the different motor types (i.e. the indices of the motors in the array)
* Maximum possible speed for the drivebase to approach
* A clipping method between a minimum and maximum value
* Normalization of wheel speeds
* A `stop()` method
* Square input method

All of the following drive types are available for use in SolversLib. All that's needed for the user to do is plug in the values to an instantiation of the object.

## Differential

A differential drive is one that has two motors or motor groups on either side of the robot. Each side acts as a connected set or motor group. There are two types of drive systems here. You can use the tank and arcade control schemes with a differential drive.

![CAD by Hrithik and Sanjay from FTC Team 16439](https://content.gitbook.com/content/53OMM4PwbbzcZqInoTsn/blobs/FJEfDjhv70gqd0XmYqdK/tank.jpg)

Creating the drivetrain object is simple:

```java
DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
```

The constructor has two parameters: left and right. For a differential drive, it considers each side as a single motor. As a result, you need to pass in a motor group object.

### Arcade

Arcade drive uses a y-value input from the controller and a value from the turn stick. We know that when the turn stick is pushed left, the right side should move forward and the left side should move backwards. Therefore, since pushing the turn stick to the left returns a negative value, it should be added to the left speed and subtracted from the right speed.

Here is how to call the method for arcade drive. An additional boolean parameter is optional, which is `squareInputs`. By default, this value is false.

```java
m_drive.arcadeDrive(forwardSpeed, turnSpeed);
```

### Tank

Tank drive uses a y-value input from the left and right sticks. The sticks control their respective side of the robot.

```java
m_drive.tankDrive(leftSpeed, rightSpeed);
```

## Holonomic

In SolversLib, we shortened holonomic drive to the generic term H-Drive. A holonomic drive is one that can move in omnidirectional space, which is why it is often called an omnidirectional drive. There are different sub-types of holonomic drivebases.

### Holonomic Drivebases

#### Three Wheel Holonomic

A three wheel holonomic drivebase, otherwise known as a "Kiwi" or "Killough," is a drivetrain with omnidirectional motion while utilizing three omniwheels.

![Rendered by Pranay from FTC 16236, CAD made by Eric from FTC 18246](https://content.gitbook.com/content/53OMM4PwbbzcZqInoTsn/blobs/zxfaBl7r4kYWPASY00gq/ftclib_kiwi_v4.png)

You can create the kiwi drive as such:

```java
// for if the wheels are oriented at 60 degrees,
// 120 degrees, and 270 degrees
HDrive kiwi_A = new HDrive(left, right, slide);

// for if the wheels are oriented at other angles
HDrive kiwi_B = new HDrive(left, right, slide,
                           leftAngle, rightAngle, slideAngle);
```

Your slide motor is generally the back of the kiwi drive and the others are self-explanatory. Note that the motor angles above are in Radians. Ensure that you're associating the correct motor/wheel with the corresponding argument as they *don't* go around the robot in order. Forward is 0 degrees.

#### X-Drive

An X-drive is a holonomic base that has four omniwheels positioned into an "X" shape as seen below.

![An X-Drive concept from VEX](https://content.gitbook.com/content/53OMM4PwbbzcZqInoTsn/blobs/ClqJdep5y8LkejWEiTKt/x-drive.jpg)

You can create the x-drive drive as such:

```java
// input motors exactly as shown below
HDrive xDrive = new HDrive(frontLeft, frontRight,
                           backLeft, backRight);
```

#### Mecanum

A mecanum drivebase is a type of holonomic drive that utilizes [mecanum wheels](https://en.wikipedia.org/wiki/Mecanum_wheel) for movement.

![A custom parallel plate mecanum drivetrain](https://content.gitbook.com/content/53OMM4PwbbzcZqInoTsn/blobs/F8FVS1hCEBOprOQZUbMc/final-drivebase-render-light.png)

For more information on mecanum drives, please watch this video:

{% embed url="<https://www.youtube.com/watch?v=8rhAkjViHEQ>" %}

You can create the mecanum drive as such:

```java
// input motors exactly as shown below
MecanumDrive mecanum = new MecanumDrive(frontLeft, frontRight,
                                        backLeft, backRight);
```

### Control Scheme

Like how the differential drive has two different control schemes, so too does holonomic. There is field-centric and robot-centric. The only difference is that there is some sort of interpolation of the robot's heading (the direction the forward face is pointing globally).

#### Robot-Centric

Robot-centric assumes that each push of the joystick is in relation to the local position of the robot—this means that whenever the user pushes the drive stick forward, the robot will drive in the direction of its front-facing side.

For all types of holonomic drive you do this by calling the .`driveRobotCentric()` method that takes the gamepad inputs and converts them into directional values.

```java
m_drive.driveRobotCentric(strafeSpeed, forwardSpeed, turnSpeed);
```

#### Field-Centric

Field-centric assumes that each push of the joystick is in relation to the global position of the robot—this means that whenever the user pushes the drive stick forward, the robot will move away from the driver no matter its orientation. This is done by rotating the direction of the joystick clockwise by an angle measurement equivalent to the global heading of the robot.

For all types of holonomic drive you do this by calling the .`driveFieldCentric()` method that takes the gamepad inputs and converts them into directional values. Additionally, you include the reading for the heading in degrees.

```java
m_drive.driveFieldCentric(strafeSpeed, forwardSpeed, turn, heading);
```

The `heading` argument is the current heading of the robot, usually from the IMU. Note that it is in *degrees* here, not radians.

### Sample

{% code title="BasicDriveOpMode.java" %}

```java
@TeleOp
public class BasicDriveOpMode extends OpMode {

    private Motor fL, fR, bL, bR;
    private MecanumDrive drive;
    private GamepadEx driverOp;

    @Override
    public void init() {
        /* instantiate motors */
        
        drive = new MecanumDrive(fL, fR, bL, bR);
        driverOp = new GamepadEx(gamepad1);
    }
    
    @Override
    public void loop() {
        drive.driveRobotCentric(
            driverOp.getLeftX(),
            driverOp.getLeftY(),
            driverOp.getRightX()
        );
    }

}
```

{% endcode %}

You can find an additional sample [here](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrivingSample.java) which is more thoroughly detailed.


# Geometry

package com.seattlesolvers.solverslib.geometry

SolversLib provides access to geometry classes taken from WPILib. Since we like copy-pasting straight from WPILib instead of linking to the [original material](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/geometry/pose.html), that's what we're gonna do.

## Translation

Translation in 2 dimensions is represented by SolversLib's`Translation2d` class. This class has an x and y component, representing the point $$(x,y)$$ or the vector $$\begin{bmatrix} x\ y \end{bmatrix}$$ on a 2-dimensional coordinate system.

You can get the distance to another `Translation2d` object by using the `getDistance(Translation2d other)`, which returns the distance to another `Translation2d` by using the Pythagorean theorem.

## Rotation

Rotation in 2 dimensions is represented by SolversLib’s `Rotation2d` class. This class has an angle component, which represents the robot’s rotation relative to an axis on a 2-dimensional coordinate system. Positive rotations are counterclockwise.

## Pose

Pose is a combination of both translation and rotation and is represented by the `Pose2d` class. It can be used to describe the pose of your robot in the field coordinate system, or the pose of objects, such as vision targets, relative to your robot in the robot coordinate system. `Pose2d` can also represent the vector $$\begin{bmatrix} x\ y\ \theta \end{bmatrix}$$ .

## Vector

A vector in 2 dimensions is represented by the `Vector2d` class. It holds an $$x$$ and a $$y$$ value similarly to a `Translation2d`. These components representing the point $$(x,y)$$ or as the matrix$$\begin{bmatrix} x\ y \end{bmatrix}$$.

Unlike a `Translation2d`, there are a few different methods and features.

## Transform and Twist

SolversLib provides 2 classes, `Transform2d`, which represents a transformation to a pose, and `Twist2d` which represents a movement along an arc. `Transform2d` and `Twist2d` all have $$x$$ , $$y$$ and $$\theta$$ components.

`Transform2d` represents a **relative** transformation. It has an translation and a rotation component. Transforming a `Pose2d` by a `Transform2d` rotates the translation component of the transform by the rotation of the pose, and then adds the rotated translation component and the rotation component to the pose. In other words, `Pose2d.plus(Transform2d)` returns $$\begin{bmatrix} x\_{p} \ y\_{p} \ \theta\_{p} \end{bmatrix} + \begin{bmatrix} \cos{\theta\_{p}} & -\sin{\theta\_{p}} & 0 \ \sin{\theta\_{p}} & \cos{\theta\_{p}} & 0 \ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\_{t} \ y\_{t} \ \theta\_{t} \end{bmatrix}$$ .

`Twist2d` represents a change in distance along an arc. For a given arc traveled, $$x$$ is the distance traveled forward as measured from the robot's perspective throughout the movement (for a differential drive, this is the arc length), $$y$$ is the distance traveled sideways from the robot's perspective (for a differential drive, this is 0), and $$\theta$$ is the change in heading.

Both classes can be used to estimate robot location. `Twist2d` is used in some of the SolversLib odometry classes to update the robot’s pose based on movement, while `Transform2d` can be used to estimate the robot’s global position from vision data.


# Controllers

package com.seattlesolvers.solverslib.controller

In SolversLib, there are controllers that can improve the motion of mechanisms in FTC. This includes PID, feedforward, SquID, and cascade control.

{% hint style="success" %}
If you wish to use any one of the controllers mentioned above directly, you can look at the individual subpages for them directly
{% endhint %}

The primary controller is an abstract class called `Controller`, which is a generic closed-loop controller. You can find the source code for the `Controller` class [here](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/controller/Controller.java), and the theory behind it on CtrlAltFtc [here](https://www.ctrlaltftc.com/introduction-to-closed-loop-control).

{% hint style="info" %}
All other controllers (PID/PIDF, FF, SquIDF, Cascade) extend this controller in some manner, and inherits its protected fields and methods. They provide added functionality that will be usually be more helpful to most users.
{% endhint %}

The `Controller` class serves as the foundation for all control algorithms in SolversLib. It provides common functionality for setpoint tracking, error calculation, tolerance checking, and output management. Concrete implementations (like PID, Feedforward, or SquID) extend this class and implement the `calculateOutput()` method with their specific control logic.

There are 3 key points that are a bit more specific for SolversLib's `Controller`:

* **Minimum Output**: When not at setpoint, the absolute value of the output will be at least `minOutput` (if it is set), preserving the sign of the correction (using `Math.signum()`)
* **Error Calculation**: Error is automatically updated whenever `setSetPoint()` is called
* **atSetpoint()**: Returns `true` only when **both position and velocity errors** are within tolerance - this is especially important for PID/PIDF classes

This class is generally used for advanced purposes, like SolversLib's[ Point-to-Point](/pathing/point-to-point) drivetrain control system.

### Using the Controller Class

in order to use SolversLib's Controller, users must first construct a `Controller` object with its only constructor:

```java
Controller controller = new Controller();
```

This constructor is implicitly called during construction of the other controllers.

#### Methods

1. **`double calculate(double pv)`**

Calculates the control output using the given measured value (process variable).

**Parameters:**

* `pv` - The current measured value

**Returns:** The control value u(t), respecting minimum output if not at setpoint

2. **`double calculate(double pv, double sp)`**

Calculates the control output with a new setpoint and measured value.

**Parameters:**

* `pv` - The current measured value
* `sp` - The desired setpoint

**Returns:** The control value u(t)

3. **`double calculate()`**

Calculates the control output using the stored measured value.

**Returns:** The control value u(t)

**Note:** Requires `measuredValue` to be set beforehand

4. **`void setSetPoint(double sp)`**

Sets the controller's target setpoint and updates error calculations.

**Parameters:**

* `sp` - The desired setpoint

**Side Effects:**

* Updates `errorVal_p` (positional error)
* Updates `errorVal_v` (velocity error)

5. **`double getSetPoint()`**

Returns the current setpoint.

**Returns:** The current target setpoint

6. **`boolean atSetPoint()`**

Checks if the controller has reached the setpoint within the defined tolerances.

**Returns:** `true` if both positional and velocity errors are within tolerance, `false` otherwise

7. **`double getPositionError()`**

Returns the current positional error e(t).

**Returns:** The difference between setpoint and measured value

8. **`double getVelocityError()`**

Returns the current velocity error e'(t).

**Returns:** The rate of change of the error

9. **`void setTolerance(double positionTolerance)`**

Sets the acceptable positional error for `atSetPoint()` checks. Velocity tolerance remains at infinity.

**Parameters:**

* `positionTolerance` - Maximum acceptable positional error

**Example:**

```java
controller.setTolerance(2.0); // Within 2 units is acceptable
```

10. **`void setTolerance(double positionTolerance, double velocityTolerance)`**

Sets both positional and velocity error tolerances.

**Parameters:**

* `positionTolerance` - Maximum acceptable positional error
* `velocityTolerance` - Maximum acceptable velocity error
*

11. **`double[] getTolerance()`**

Returns the current tolerance settings.

**Returns:** Array containing `[positionTolerance, velocityTolerance]`

12. **`void reset()`**

Resets the controller state by clearing previous error and timestamp values.

**Use Case:** Call this when switching between different control tasks or when re-enabling a disabled controller

13. **`Controller setMinimumOutput(double minOutput)`**

Enforces a minimum output magnitude when the controller is not at the setpoint. This prevents the controller from applying insignificantly small corrections.

**Parameters:**

* `minOutput` - Minimum absolute value of the output (sign is preserved)

**Returns:** The controller instance (for method chaining)

14. **`double getPeriod()`**

Returns the time period between controller calculations.

**Returns:** The period in seconds (or your time unit)

### Protected Methods

`abstract double calculateOutput(double pv)`

This is the core control algorithm that is implemented by subclasses.

**Parameters:**

* `pv` - The measured value

**Returns:** The raw control output before minimum output enforcement

**Note:** This method is called internally by `calculate()`. Subclasses override this to implement specific control strategies (PID, feedforward, etc.).


# PID and PIDF

package com.seattlesolvers.solverslib.controller

## PID Control

The following post from Noah in the [FTC Discord](https://discord.gg/first-tech-challenge) best explains PID control.

> You'll hear the term PID Controller used a lot (F is tacked on sometimes) in robotics. It's relatively simple and pretty effective at a lot of simple tasks. A PID controller is a form of "closed loop control." This basically just means you're altering an input to some "plant" based on feedback. This concept applies to a wide range of actions but we'll take a look at velocity PID control as that is what's relevant for this year's game. So say you have a goBILDA 3:1 1620RPM motor powering a flywheel. You want that flywheel to spin at a constant speed to ensure consistency between your shots. So you run a `motor.setPower(0.5)` which sends 50% of 12v to the motor. The motor is getting a 6v signal (technically not true bc of PWM but that's another topic). The motor should be running at 810 RPM right? That's 50% of 1620RPM. Chances are, it's not actually running at this speed. Motors have +- 10% tolerance between them. The voltage-torque curve isn't linear. Or there is something resisting the motor (like the inertia of the flywheel) so it takes extra power to get it up to that speed. So how do we actually ensure that our motor is running at exactly 810RPM? Most FTC motors come with an encoder built it. This allows us to measure the velocity of the output shaft. So with the encoder all hooked up, we know that our motor isn't spinning as fast as we want it to. But how do we actually correct for this? You slap a PID Controller on it. A PID controller is a pretty basic controller (despite the daunting equation when you look it up) that basically responds to your the difference between your measured velocity and desired velocity (error) and will add more or less power based on error. You just check the velocity every loop, feed the value in the controlled, and it gives you the power you want to set it to the desired velocity.

For information on the theory behind PID control, we recommend reading [this page](https://gm0.org/en/stable/docs/software/control-loops.html) in gm0 and [this page](https://www.ctrlaltftc.com/the-pid-controller) in CtrlAltFtc.&#x20;

The following video does a good job explaining each gain:

{% embed url="<https://www.youtube.com/watch?v=XfAt6hNV8XM>" %}

Our base class is `PIDFController` for the SolversLib PID control scheme. This class performs the calculations for PIDF, which are proportional, integral, derivative, and feedforward values. The additional F term is an additional gain for creating offset, for purposes like maintaining a position, counteracting weight/gravity, or overcoming friction.

### Using the PIDFController Class

#### Constructing a PIDFController

In order to use SolversLib's PIDF control functionality, users must first construct a `PIDFController` object with the desired gains:

```java
// Creates a PIDFController with gains kP, kI, kD, and kF
PIDFController pidf = new PIDFController(kP, kI, kD, kF);

/*
 * Here are the constructors for the other controllers
 */
PIDController pid = new PIDController(kP, kI, kD);
PDController pd = new PDController(kP, kD);
PController p = new PController(kP);
```

You can also pass in an additional two parameters: the setpoint and previous value. The default values for these are 0.

You can also change the gain constants even after creating the controller object.

```java
// set our gains to some value
pidf.setP(0.37);
pidf.setI(0.05);
pidf.setD(1.02);

// get our gain constants
kP = pidf.getP();
kI = pidf.getI();
kD = pidf.getD();

// set all gains
pidf.setPIDF(kP, KI, kD, 0.7);

// get all gain coefficients
double[] coeffs = pidf.getCoefficients();
kP = coeffs[0];
kI = coeffs[1];
kD = coeffs[2];
kF = coeffs[3];
```

These gains must be tuned. An example of tuning PID can be found on [this page](https://www.learnroadrunner.com/drive-velocity-pid-tuning.html) of Learn Road Runner.

#### Using the Feedback Loop Output

The `calculate()` method should be called each iteration of the control loop. The controller uses timestamps to calculate the difference in time between each call of the method, which means it adjusts based on the loop time. You can obtain the cycle time of your current loop iteration by calling `getPeriod()`.

Using the constructed `PIDFController` is simple: call the `calculate()` method from the main loop.

```java
// Calculates the output of the PIDF algorithm based on sensor
// readings. Requires both the measured value
// and the desired setpoint
double output = pidf.calculate(
    motor.getCurrentPosition(), setpoint
);

/*
 * A sample control loop for a motor
 */
PController pController = new PController(kP);

// We set the setpoint here.
// Now we don't have to declare the setpoint
// in our calculate() method arguments.
pController.setSetPoint(1200);

// perform the control loop
/*
 * The loop checks to see if the controller has reached
 * the desired setpoint within a specified tolerance
 * range
 */
while (!pController.atSetPoint()) {
  double output = pController.calculate(
    m_motor.getCurrentPosition()  // the measured value
  );
  m_motor.setVelocity(output);
}
m_motor.stopMotor(); // stop the motor

// NOTE: motors have internal PID control
```

#### Checking Errors

The methods `getPositionError()` and `getVelocityError()` are named assuming that the loop is controlling a position - for a loop that is controlling a velocity, these return the velocity error and the acceleration error, respectively.

The current error of the measured process variable is returned by the `getPositionError()` function, while its derivative is returned by the `getVelocityError()` function.

#### Specifying and Checking Tolerances

If only a position tolerance is specified, the velocity tolerance defaults to infinity.

As above, “position” refers to the process variable measurement, and “velocity” to its derivative - thus, for a velocity loop, these are actually velocity and acceleration, respectively.

Occasionally, it is useful to know if a controller has tracked the setpoint to within a given tolerance - for example, to determine if a command should be ended, or (while following a motion profile) if motion is being impeded and needs to be re-planned.

To do this, we first must specify the tolerances with the `setTolerance()` method; then, we can check it with the `atSetPoint()` method.

```java
// Sets the error tolerance to 5, and the error derivative
// tolerance to 10 per second
pidf.setTolerance(5, 10);

// Returns true if the error is less than 5 units, and the
// error derivative is less than 10 units
pidf.atSetPoint();
```

#### Resetting the Controller

It is sometimes desirable to clear the internal state (most importantly, the integral accumulator) of a `PIDFController`, as it may be no longer valid (e.g. when the `PIDFController` has been disabled and then re-enabled). This can be accomplished by calling the `reset()` method.


# Feedforward Helpers

package com.seattlesolvers.solverslib.controller

## Feedforward Control

So far, we’ve used feedback control for reference tracking (making a system’s output follow a desired reference signal). While this is effective, it’s a reactionary measure; the system won’t start applying control effort until the system is already behind. If we could tell the controller about the desired movement and required input beforehand, the system could react quicker and the feedback controller could do less work. A controller that feeds information forward into the plant like this is called a feedforward controller.

A feedforward controller injects information about the system’s dynamics (like a mathematical model does) or the intended movement. Feedforward handles parts of the control actions we already know must be applied to make a system track a reference, then feedback compensates for what we do not or cannot know about the system’s behavior at runtime.

There are two types of feedforwards: model-based feedforward and feedforward for unmodeled dynamics. The first solves a mathematical model of the system for the inputs required to meet desired velocities and accelerations. The second compensates for unmodeled forces or behaviors directly so the feedback controller doesn’t have to. Both types can facilitate simpler feedback controllers. We’ll cover several examples below.

SolversLib provides a number of classes to help users implement accurate feedforward control for their mechanisms. In many ways, an accurate feedforward is more important than feedback to effective control of a mechanism. Since most FTC mechanisms closely obey well-understood system equations, starting with an accurate feedforward is both easy and hugely beneficial to accurate and robust mechanism control.

SolversLib currently provides the following three helper classes for feedforward control. The feedforward components will calculate outputs in units determined by the units of the user-provided feedforward gains. Users *must* take care to keep units consistent as it does not have a type-safe unit system.

### SimpleMotorFeedforward

The `SimpleMotorFeedforward` class calculates feedforwards for mechanisms that consist of permanent-magnet DC motors with no external loading other than friction and inertia, such as flywheels and robot drives.

To create a `SimpleMotorFeedforward`, simply construct it with the required gains:

```java
// Create a new SimpleMotorFeedforward with gains kS, kV, and kA
SimpleMotorFeedforward feedforward =
    new SimpleMotorFeedforward(kS, kV, kA);
```

Please note that the `kA` value is optional. If the mechanism does not have much inertia, then it is not required.

To calculate the feedforward, simply call the `calculate()` method with the desired motor velocity and acceleration:

```java
// Calculates the feedforward for a velocity of 10 units/second
// and an acceleration of 20 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20);
```

### ArmFeedforward

The `ArmFeedforward` class calculates feedforwards for arms that are controlled directly by a permanent-magnet DC motor, with external loading of friction, inertia, and mass of the arm. This is an accurate model of most arms in FTC.

To create an `ArmFeedforward`, simply construct it with the required gains:

```java
// Create a new ArmFeedforward with gains kS, kCos, kV, and kA
ArmFeedforward feedforward = new ArmFeedforward(kS, kCos, kV, kA);
```

To calculate the feedforward, simply call the `calculate()` method with the desired arm position, velocity, and acceleration:

```java
// Calculates the feedforward for a position of 1 units,
// a velocity of 2 units/second, and
// an acceleration of 3 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(1, 2, 3);
```

### ElevatorFeedforward

The `ElevatorFeedforward` class calculates feedforwards for elevators that consist of permanent-magnet DC motors loaded by friction, inertia, and the mass of the elevator. This is an accurate model of most elevators in FTC.

To create a `ElevatorFeedforward`, simply construct it with the required gains:

```java
// Create a new ElevatorFeedforward with gains kS, kG, kV, and kA
ElevatorFeedforward feedforward = new ElevatorFeedforward(
    kS, kG, kV, kA
);
```

To calculate the feedforward, simply call the `calculate()` method with the desired motor velocity and acceleration:

```java
// Calculates the feedforward for a position of 10 units,
// velocity of 20 units/second,
// and an acceleration of 30 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20, 30);
```

### Using Feedforward to Control a Mechanism

Feedforward control can be used entirely on its own, without a feedback controller. This is known as “open-loop” control, and for many mechanisms (especially robot drives) can be perfectly satisfactory. A `SimpleMotorFeedforward` might be employed to control a robot drive as follows:

```java
public void tankDriveWithFeedforward(double leftVelocity,
                                     double rightVelocity) {
  leftMotor.set(feedforward.calculate(leftVelocity));
  rightMotor.set(feedforward.calculate(rightVelocity));
}
```


# SquIDF

package com.seattlesolvers.solverslib.controller

A `SquIDF` controller is a variation of the `PIDF` controller that square roots the positional error term. Effectively, it extends the `PIDF` class, and overrides the `calculateOutput` method. This modification changes how the controller responds to errors of different magnitudes.

#### Key Differences from PIDF

| Aspect                    | PIDF                        | SquIDF               |
| ------------------------- | --------------------------- | -------------------- |
| **Proportional Response** | Linear with error           | Square root of error |
| **Large Errors**          | Stronger correction         | Moderate correction  |
| **Small Errors**          | Generally weaker correction | Stronger correction  |
| **Response Curve**        | Straight line               | Curved (sub-linear)  |

`SquIDF` helps reduces overshoot (since the square root dampens aggressive corrections when initial error is high), and generally gives a smoother approach. However, like a PID/PIDF, tuning is critical. The controller is only as good as you can tune it.

#### Usage Example

```java
// Create SquIDF controller with same gains as PIDF
SquIDFController squidfController = new SquIDFController(
    1.0,  // kP
    0.1,  // kI
    0.05, // kD
    0.2   // kF
);

// Use just like a PIDF controller
squidfController.setSetPoint(targetPosition);
double output = squidfController.calculate(currentPosition);
```

#### Tuning Notes

* **kP values** will typically need to be **higher** than in PIDF since √(e) < e for errors > 1
* Start with your PIDF gains and increase kP gradually
* The I, D, and F terms work pretty much identically to PIDF


# Cascade Control

package com.seattlesolvers.solverslib.controller

A `CascadeController` chains two controllers together into a single closed-loop system. The **primary** (outer) controller runs on position: it compares the measured position against the position setpoint and outputs a *velocity command*. The **secondary** (inner) controller runs on velocity: it compares the mechanism's measured velocity against that velocity command and outputs the final control value (typically motor power).

This nested structure is called cascade control. The inner velocity loop reacts quickly to disturbances (battery sag, friction, gravity, a game element landing in your intake) before they ever show up as position error, while the outer position loop only has to steer a well-behaved velocity loop. The result is generally smoother, more consistent motion than a single position PID - especially on heavy arms and high-speed slides.

You can find the source code for the `CascadeController` class [here](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/controller/CascadeController.java), and the theory behind closed-loop control on CtrlAltFtc [here](https://www.ctrlaltftc.com/).,&#x20;

### When to use this over a standard PIDF?

* **Heavy arms and lifts:** the inner velocity loop fights gravity and load changes immediately, instead of waiting for position error to build up
* **Fast(er) slides:** commanding velocity rather than raw power gives controlled acceleration and less slamming at the ends of travel
* **Consistency across battery voltage:** a velocity loop compensates for voltage drop automatically, so the same setpoint behaves the same at 14V and at 12V

If a single well-tuned PIDF already does the job for your mechanism, you probably don't need a cascade. Consider using a dual PIDF controller (one for large error, one for small error) before this.

#### Constructing a CascadeController

`CascadeController` takes any two SolversLib `Controller` objects - most commonly a `PIDController` or `PIDFController` for each loop:

```java
// Outer loop: position error (ticks) -> velocity command (ticks per second)
PIDController positionController = new PIDController(kP_pos, kI_pos, kD_pos);

// Inner loop: velocity error (ticks per second) -> motor power
PIDFController velocityController = new PIDFController(kP_vel, kI_vel, kD_vel, kF_vel);

CascadeController cascade = new CascadeController(positionController, velocityController);
```

Because the two gain sets multiply through the cascade, the outer controller's output is in *velocity units*, and the inner controller's gains convert velocity error into power. Expect the outer `kP` to be much larger than a standalone position PID's, and the inner gains to be small.

#### Setting Setpoints

```java
// Target a position; velocity setpoint is cleared to 0
cascade.setSetPoint(1200);

// Target a position AND cruise toward it at a desired velocity
cascade.setSetPoints(1200, 800); // position setpoint, velocity setpoint
```

The velocity setpoint from `setSetPoints()` is added to the primary controller's output before it reaches the inner loop, so it acts as a velocity feedforward - useful for following motion profiles, where each loop you feed in the profile's instantaneous target position and target velocity.

{% hint style="warning" %}
`setSetPoint(sp)` resets the velocity setpoint back to 0. If you are using a nonzero velocity setpoint, keep using `setSetPoints()`.
{% endhint %}

#### Usage Example

A lift driven to a scoring position with a cascade of two controllers:

```java
PIDController positionController = new PIDController(8.0, 0, 0.1);
PIDFController velocityController = new PIDFController(0.0004, 0.00004, 0, 0.0002);

CascadeController cascade = new CascadeController(positionController, velocityController);

cascade.setMaxOutput(1.0);
cascade.setTolerance(15);

waitForStart();

cascade.setSetPoint(1200);

while (opModeIsActive()) {
    double power = cascade.calculate(lift.getCurrentPosition());
    lift.setPower(power);

    telemetry.addData("measured velocity", cascade.getMeasuredVel());
    telemetry.addData("at target", cascade.atSetPoint());
    telemetry.update();
}
```

#### Measuring Velocity

`getMeasuredVel()` returns the velocity the controller measured on its most recent `calculate()` call, in position units per second. This is handy for telemetry, logging, and tuning the inner loop.

```java
double ticksPerSecond = cascade.getMeasuredVel();
```

#### Resetting the Controller

`reset()` clears the measured velocity and timestamps, and also resets both the primary and secondary controllers, so any wound-up integral term in either loop is discarded. This is useful for when you need to re-enable the mechanism or switching between control tasks.

```java
cascade.reset();
```


# Motion Profiling

packages com.seattlesolvers.solverslib.trajectory, .controller.wpilibcontroller, and .command

A plain PID loop only reacts to error. If you jump a lift's setpoint from 0 to 1200 ticks in one instant, the error is suddenly huge, the controller slams maximum power into the motor, and the result is jerky motion, belt skipping, and overshoot. Motion profiling fixes this by moving the *setpoint* smoothly instead of teleporting it: a profile generates a stream of intermediate setpoints between where the mechanism is and where you want it to be, so the PID loop only ever chases a small, achievable error.

The most common profile in FTC is the **trapezoid profile**, which splits a movement into three phases:

1. **Accelerate** at a constant rate up to a maximum velocity
2. **Cruise** at that maximum velocity
3. **Decelerate** at a constant rate, arriving at the goal with the desired end velocity (usually zero)

Plot velocity against time and the graph forms a trapezoid, hence the name. For short movements the mechanism may never reach cruise velocity, in which case the trapezoid degenerates into a triangle - the classes below handle this automatically.

{% hint style="info" %}
These classes are ports of older versions of WPILib's `TrapezoidProfile` and `ProfiledPIDController`. Note that recent WPILib versions changed the `TrapezoidProfile` API, so older WPILib documentation matches SolversLib more closely than current WPILib docs. For the theory behind motion profiling, we recommend [this page](https://www.ctrlaltftc.com/advanced/motion-profiling) on CtrlAltFtc.
{% endhint %}

## The TrapezoidProfile Class

[`TrapezoidProfile`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java) represents a single trapezoidal movement and lets you sample the position and velocity the mechanism *should* have at any point in time.

### Constraints and States

The profile is described by two small nested classes:

* `TrapezoidProfile.Constraints` holds the `maxVelocity` and `maxAcceleration` the mechanism is allowed to reach.
* `TrapezoidProfile.State` holds a `position` and a `velocity` - a snapshot of where the mechanism is (or should be) and how fast it is moving.

```java
// The lift may travel at up to 1800 ticks/sec, accelerating at 2400 ticks/sec^2
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1800, 2400);

// End at 1200 ticks, at rest
TrapezoidProfile.State goal = new TrapezoidProfile.State(1200, 0);

// Start from the lift's current position, at rest
TrapezoidProfile.State initial = new TrapezoidProfile.State(liftMotor.getCurrentPosition(), 0);
```

You can use any units you like (encoder ticks, inches, radians) as long as you are consistent: velocities are units per second and accelerations are units per second squared.

### Creating and Sampling a Profile

```java
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, initial);

// If the initial state is omitted, the profile starts at rest at position 0
TrapezoidProfile profile2 = new TrapezoidProfile(constraints, goal);
```

`calculate(t)` returns the `State` the mechanism should be at `t` seconds after the start of the profile:

```java
ElapsedTime timer = new ElapsedTime();

while (opModeIsActive() && !profile.isFinished(timer.seconds())) {
    TrapezoidProfile.State setpoint = profile.calculate(timer.seconds());
    // Feed setpoint.position into your own position controller here.
    // setpoint.velocity is also available, e.g. for a velocity feedforward.
}
```

A few other useful methods:

* `totalTime()` returns how long the whole profile takes, in seconds.
* `isFinished(t)` returns true once `t` has passed the profile's total time.
* `timeLeftUntil(target)` returns the time remaining until the profile reaches a target distance.

Most teams never need to sample a profile by hand, though - the `ProfiledPIDController` below does it for you.

## The ProfiledPIDController Class

[`ProfiledPIDController`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java) wraps a trapezoid profile around a normal PID loop. Instead of giving the controller a setpoint, you give it a **goal**. On every call to `calculate()`, the controller builds a profile from its current intermediate setpoint toward the goal, steps that profile forward by the measured loop period, and then runs regular PID math against the new intermediate setpoint. The effect is that the setpoint glides toward the goal under your velocity and acceleration constraints, and the PID loop just has to keep up.

### Constructing a ProfiledPIDController

```java
ProfiledPIDController controller = new ProfiledPIDController(
    kP, kI, kD,
    new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration)
);
```

The gains can be changed later with `setP()`, `setI()`, `setD()`, or `setPID()`, and the constraints with `setConstraints()`.

### Goal vs. Setpoint

Because the controller manages its own intermediate setpoint, it distinguishes between the two:

* `setGoal(1200)` or `setGoal(new TrapezoidProfile.State(1200, 0))` sets where you ultimately want to end up. The `double` overload assumes a goal velocity of zero.
* `getSetpoint()` returns the current intermediate `State` along the profile.
* `atSetpoint()` checks whether the PID loop is tracking the intermediate setpoint within the tolerance set by `setTolerance()`.
* `atGoal()` checks whether the mechanism has actually reached the goal (at the setpoint *and* the setpoint has reached the goal).

### Using the Controller in a Loop

```java
ProfiledPIDController controller = new ProfiledPIDController(
        0.01, 0, 0.0002,
        new TrapezoidProfile.Constraints(1800, 2400)
);
controller.setTolerance(15);

// Start the profile from the lift's actual position so the setpoint does not jump
controller.reset(liftMotor.getCurrentPosition());
controller.setGoal(1200);

while (opModeIsActive() && !controller.atGoal()) {
    double output = controller.calculate(liftMotor.getCurrentPosition());
    liftMotor.set(output);
}
liftMotor.stopMotor();
```

You can also pass the goal (and even new constraints) directly into `calculate(measurement, goal)` each loop, which is handy when the goal changes on the fly - for example, a lift height chosen by the driver.

{% hint style="warning" %}
The intermediate setpoint persists between movements. Always call `reset(measuredPosition)` (or `reset(measuredPosition, measuredVelocity)`) before starting a new movement - otherwise the first `calculate()` call plans a profile from a stale setpoint and the output can jump.
{% endhint %}

## Command-Based Wrappers

If you use SolversLib's command-based framework, two ready-made commands wrap the classes above.

### ProfiledPIDCommand

[`ProfiledPIDCommand`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java) runs a `ProfiledPIDController` inside a command. Each `execute()` it reads the measurement supplier, calls `calculate()`, and passes both the output and the current profile setpoint to your `BiConsumer` - the setpoint's velocity is useful for adding a feedforward term. On `initialize()` it automatically resets the controller to the current measurement, and on `end()` it sends an output of zero.

The goal can be given as a fixed `double` or `TrapezoidProfile.State`, or as a `DoubleSupplier`/`Supplier<State>` that is re-read every loop.

```java
// lift is a user-defined Subsystem with getPosition() and setPower() methods
ProfiledPIDCommand raiseLift = new ProfiledPIDCommand(
        new ProfiledPIDController(0.01, 0, 0.0002,
                new TrapezoidProfile.Constraints(1800, 2400)),
        lift::getPosition,                            // measurement
        1200,                                         // goal (goal velocity is implicitly zero)
        (output, setpoint) -> lift.setPower(output),  // use the output
        lift                                          // required subsystem
);

// The command runs forever by default, so add an end condition
raiseLift.interruptOn(() -> raiseLift.getController().atGoal()).schedule();
```

To add more complex exit conditions or behavior, subclass `ProfiledPIDCommand` and override `isFinished()`.

### TrapezoidProfileCommand

[`TrapezoidProfileCommand`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/TrapezoidProfileCommand.java) runs a raw `TrapezoidProfile` on a timer, feeding each sampled `State` to a `Consumer`. It finishes automatically once the profile's total time has elapsed. This is a good fit when the mechanism already has its own position controller and you just need to stream setpoints to it.

```java
TrapezoidProfile profile = new TrapezoidProfile(
        new TrapezoidProfile.Constraints(1800, 2400),
        new TrapezoidProfile.State(1200, 0),               // goal
        new TrapezoidProfile.State(lift.getPosition(), 0)  // initial
);

TrapezoidProfileCommand followProfile = new TrapezoidProfileCommand(
        profile,
        state -> lift.setTargetPosition(state.position),
        lift
);
```

Note that the profile is baked in when the command is constructed, so build the command right before scheduling it (or in a subclass) so the initial state matches the mechanism's actual position at that moment.


# Utility Functions

package com.seattlesolvers.solverslib.util;

SolversLib comes with many different Utility Functions:

* [Look Up Tables](https://docs.seattlesolvers.com/features/util#what-is-a-look-up-table)
* [Timing Functions](https://docs.seattlesolvers.com/features/util#timing-functions)
* [Math Utilities](https://docs.seattlesolvers.com/features/util#math-utilities)
* [Directional Enums](https://docs.seattlesolvers.com/features/util#directional-enums)

## What is a Look Up Table?

A look up table or LUT for short is used to store values and be able to quickly recall them.

The SolversLib provides 2 different variations of look up tables. In this year's game they can be used to store different set and tested velocities or angles. You can either retrieve the closest reference or you can interpolate through them.

## LUT (Look Up Table)

Provides a way to store values in a table to quickly retrieve them. For example, this might be used to store different speeds or angles based on certain distances. This class allows you to find the closest entry to the input.

For example if you enter:

| Input | Output |
| :---: | :----: |
|   0   |    0   |
|   1   |    1   |
|   2   |    1   |

When you request 1.1, it will return 1.

### Example Usage:

```java
import com.seattlesolvers.solverslib.util.LUT;

LUT<Double, Double> speeds = new LUT<Double, Double>()
{{
    add(5.0, 1.0);
    add(4.0, 0.9);
    add(3.0, 0.75);
    add(2.0, 0.5);
    add(1.0, 0.2);
}};

double distance = odometry.getPose().getTranslation().getDistance(new Translation2d(5, 10));
shooter.set(speeds.getClosest(distance));
```

## InterpLUT (Interpolated Look Up Table)

Provides a way to fill in the gaps in the data. Similarly to the LUT above, this allows you to add data points and retrieve a data point given an output. The difference between a normal LUT and InterpLUT is that the interpolated LUT uses math to fill in all the gaps. Effectively generating filler data based on the data around it.

### Example Usage:

```java
import com.seattlesolvers.solverslib.util.InterpLUT;

//Init the Look up table
InterpLUT lut = new InterpLUT();

//Adding each val with a key
lut.add(1.1, 0.2);
lut.add(2.7, .5);
lut.add(3.6, 0.75);
lut.add(4.1, 0.9);
lut.add(5, 1);
//generating final equation
lut.createLUT();

double distance = odometry.getPose().getTranslation().getDistance(new Translation2d(5, 10));
shooter.set(lut.get(distance));

//getting the velo required and passing it to the shooter.
```

## Timing Functions

SolversLib provides its timing utilities through the nested classes of the [Timing](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java) class: a `Stopwatch` for measuring elapsed and loop times, a `Timer` for counting down a fixed duration, and a `Rate` for limiting how often fast code paths run. They are lightweight alternatives to the SDK's `ElapsedTime` for quick uses.

{% hint style="warning" %}
A new `Stopwatch` or `Timer` is created **paused** at 0 — call `.start()` to begin timing.
{% endhint %}

### Stopwatch

A `Stopwatch` measures elapsed time. It can be created with a `TimeUnit`, or defaults to seconds:

```java
import com.seattlesolvers.solverslib.util.Timing;
import java.util.concurrent.TimeUnit;

Timing.Stopwatch stopwatch = new Timing.Stopwatch();                          // seconds
Timing.Stopwatch preciseStopwatch = new Timing.Stopwatch(TimeUnit.MILLISECONDS);
```

| Function                  | Return Type | Description                                                          |
| ------------------------- | ----------- | -------------------------------------------------------------------- |
| `stopwatch.start()`       | void        | Starts (or restarts) the stopwatch                                   |
| `stopwatch.start(paused)` | void        | Restarts the stopwatch, optionally leaving it paused                 |
| `stopwatch.pause()`       | void        | Pauses the stopwatch, freezing `elapsedTime()`                       |
| `stopwatch.resume()`      | void        | Resumes a paused stopwatch                                           |
| `stopwatch.elapsedTime()` | long        | Returns the elapsed (unpaused) time, in the constructor's `TimeUnit` |
| `stopwatch.deltaTime()`   | long        | Returns the time since `start()` or the last `deltaTime()` call      |
| `stopwatch.isTimerOn()`   | boolean     | Returns whether the stopwatch is running (started and not paused)    |

`deltaTime()` makes measuring loop times easy:

```java
Timing.Stopwatch loopTimer = new Timing.Stopwatch(TimeUnit.MILLISECONDS);
loopTimer.start();

while (opModeIsActive()) {
    // robot code...

    telemetry.addData("loop time (ms)", loopTimer.deltaTime());
    telemetry.update();
}
```

### Timer

A `Timer` extends `Stopwatch` to count down a fixed length, so every `Stopwatch` method above works on a `Timer` too. It is created with a length, or a length and a `TimeUnit` (defaulting to seconds):

```java
Timing.Timer timer = new Timing.Timer(30);                              // 30 seconds
Timing.Timer preciseTimer = new Timing.Timer(500, TimeUnit.MILLISECONDS);
```

On top of the `Stopwatch` methods, it adds:

| Function                | Return Type | Description                                                                |
| ----------------------- | ----------- | -------------------------------------------------------------------------- |
| `timer.remainingTime()` | long        | Returns the time left until the timer is done                              |
| `timer.done()`          | boolean     | Returns whether at least the timer's length of (unpaused) time has elapsed |

```java
Timing.Timer timer = new Timing.Timer(500, TimeUnit.MILLISECONDS);
timer.start();

while (!timer.done()) {
    // e.g. wait for a servo to physically reach its position
}
```

### Rate (Refresh Rate Timer)

A `Rate` limits how often something runs — for example, capping hardware reads/writes or telemetry updates. It only works in milliseconds and starts counting when created.

`Rate` is a non-static inner class of `Timing`, so it is constructed through a `Timing` instance:

```java
Timing.Rate rate = new Timing().new Rate(100); // 100 ms interval

// in your loop
if (rate.atTime()) {
    // runs only when at least 100 ms have passed since the last check
}
```

* `atTime()`: returns whether at least the rate's interval has passed since the last `atTime()` (or `reset()`) call, and restarts the clock either way.
* `reset()`: restarts the interval.

{% hint style="warning" %}
`atTime()` restarts its clock on **every** call, even when it returns `false`. If you poll it much faster than its interval, it will never return `true` — call it only once per loop, and keep the interval close to (or below) your loop time.
{% endhint %}

## Math Utilities

SolversLib provides its math utilities through the [MathUtils](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/util/MathUtils.java) class. Every method is `static`, so you never create a `MathUtils` object — just import the class and call the methods on it directly:

```java
import com.seattlesolvers.solverslib.util.MathUtils;
```

### Clamp

`clamp` lets you restrict a value to a certain max and min and is usable in double and int.

**Example Usage:**

Double Method:

```java
import com.seattlesolvers.solverslib.util.MathUtils;

double ValueToClamp;
double LowestPossibleValue;
double HighestPossibleValue;

double OutputVal = MathUtils.clamp(ValueToClamp,
                         LowestPossibleValue,
                         HighestPossibleValue);
```

Int Method:

```java
import com.seattlesolvers.solverslib.util.MathUtils;

int ValueToClamp;
int LowestPossibleValue;
int HighestPossibleValue;

int OutputVal = MathUtils.clamp(ValueToClamp,
                         LowestPossibleValue,
                         HighestPossibleValue);
```

### Round

`round(double number, int places)` rounds a number to the given amount of decimal places (rounding half up), which is handy for cleaning up telemetry values.

```java
double rounded = MathUtils.round(3.14159, 2); // 3.14
```

### Angle Normalization

These methods wrap an angle back into a standard range, which is useful for heading math — for example, making sure your turn-to-angle code always takes the shortest way around. The boolean parameter picks the output range: `true` gives 0 to 360° (or 0 to 2π), while `false` gives -180° to 180° (or -π to π).

| Method                                                                 | Description                                                                                 |
| ---------------------------------------------------------------------- | ------------------------------------------------------------------------------------------- |
| `normalizeAngle(double angle, boolean zeroToMax, AngleUnit angleUnit)` | Normalizes an angle in the unit given by the FTC SDK's `AngleUnit` (`DEGREES` or `RADIANS`) |
| `normalizeDegrees(double angle, boolean zeroToFull)`                   | Shortcut for `normalizeAngle` with `AngleUnit.DEGREES`                                      |
| `normalizeRadians(double angle, boolean zeroToFull)`                   | Shortcut for `normalizeAngle` with `AngleUnit.RADIANS`                                      |
| `returnMaxForAngleUnit(AngleUnit angleUnit)`                           | Returns the full-circle value for the unit (360 for degrees, 2π for radians)                |

```java
double heading = MathUtils.normalizeDegrees(450, true);  // 90.0
double error = MathUtils.normalizeDegrees(190, false);   // -170.0
```

### Signed Square Root

`sqrtWithSig(double val)` returns the square root of the absolute value while keeping the original sign — useful for shaping joystick inputs without losing direction.

```java
double shaped = MathUtils.sqrtWithSig(-0.25); // -0.5
```

## Directional Enums

SolversLib comes with multiple directional enums for all your directional needs! You can use these for any autonomous or TeleOP States or anything you want!

| Direction | Index |
| --------- | ----- |
| LEFT      | 0     |
| RIGHT     | 1     |
| UP        | 2     |
| DOWN      | 3     |
| FORWARD   | 4     |
| BACKWARDS | 5     |

You can find some examples in the [sample folder](https://github.com/FTC-23511/SolversLib/tree/master/examples/src/main/java/org/firstinspires/ftc/teamcode).


# Pedro Commands

package com.seattlesolvers.solverslib.pedroCommand

One of SolversLib's modern features is easy integration with [Pedro Pathing](< https://pedropathing.com/>), a popular path following library for Autonomous. To use this, make sure you have both the [pedroPathing module installed](https://docs.seattlesolvers.com/pedro-pathing/pages/pfxSCrt0ymNmrj0kRXOE#build.gradle-teamcode) as well as the [Pedro Pathing library installed](https://pedropathing.com/docs/pathing). This dependency is completely seperate from core, meaning that it is not installed by default, and should only be used if you are using Pedro Pathing.

SolversLib includes commands for using Pedro Pathing's Follower class, allowing you to fully use command base in your Autonomous OpModes.

These commands are for following Pedro Pathing's Path and PathChain classes, not SolversLib's Path class.&#x20;


# FollowPathCommand

import com.seattlesolvers.solverslib.pedroCommand.FollowPathCommand;

This command allows you to easily follow a Path or PathChain.

If a Path is supplied, it will simply convert it to a PatchChain first, and then follow that.

It has four parameters, with the first two being mandatory:

1. Pedro Pathing's Follower (which controls the robot movement)&#x20;
2. The Path or PathChain to follow

```java
new FollowPathCommand(follower, pathChain)
```

3. An optional boolean parameter called holdEnd that decides whether or not the robot should hold its position at the end of the Path (default value is true if not supplied)

```java
new FollowPathCommand(follower, pathChain, true)
```

4. An optional double parameter called maxPower that sets the maximum power the robot will run at for the path

```java
new FollowPathCommand(follower, pathChain, true, 0.5)
```

You can use a decorater to set the globalMaxPower for the follower as follows:

```java
new FollowPathCommand(follower, pathChain).setGlobalMaxPower(0.5)
```

Setting the Global Maximum Power sets the maximum power `globalMaxPower` for all future paths (unless rewritten again). However, setting the `maxPower` as a parameter in `FollowPathCommand` overwrites `globaMaxPower` for that path only.   &#x20;

To see how you can use this command in a [`CommandOpMode`](/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java). For usage in a full Autonomous Program, look at this [example](https://github.com/FTC-23511/SolversLib/blob/public-beta/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroAutoSample.java), and for a full TeleOp Program, at this [example](https://github.com/FTC-23511/SolversLib/blob/public-beta/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroTeleOpSample.java).


# HoldPointCommand

package com.seattlesolvers.solverslib.pedropathing

This command calls Pedro Pathing's [follower.holdPoint(Pose)](https://github.com/Pedro-Pathing/PedroPathing/blob/main/core/src/main/java/com/pedropathing/follower/Follower.java#L207), which allows you to easily hold a new Point (the Pose parameter is converted into a Point under the hood).

It has three mandatory parameters:

* Pedro Pathing's Follower (which controls the robot movement)
* The Pose to hold
* A boolean parameter called isFieldCentric that decides whether the move should be field centric or robot centric (based off the follower's position at the time of scheduling the command)

```java
HoldPointCommand(Follower follower, Pose pose, boolean isFieldCentric)
```

The following robot centric movements for Pedro Pathing's default coordinate system should be true assuming that the robot is facing forwards to the long side on the submersible's blue alliance side:

* `Pose.getX()`: -Y is forwards, +Y is backwards
* `Pose.getY()`: +X is left, -X is right
* `Pose.getHeading()`: Heading is in radians, +heading turns left and -heading turns right

To see how you can use this command in a [`CommandOpMode`](/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java).


# TurnCommand

package com.seattlesolvers.solverslib.pedropathing

This command calls Pedro Pathing's [follower.turn(radians, isLeft)](https://github.com/Pedro-Pathing/PedroPathing/blob/main/core/src/main/java/com/pedropathing/follower/Follower.java#L665), which allows you to easily turn a certain amount of radians (or degrees) in place.

It has four parameters, with the first three being mandatory:

1. Pedro Pathing's Follower (which controls the robot movement)
2. The angle to turn by (default is Radians)
3. Whether or not the robot should turn left (counterclockwise)

```java
TurnCommand(Follower follower, double angle, boolean isLeft)

// Example
new TurnCommand(follower, Math.PI / 2, true)
```

4. An optional parameter for a custom `AngleUnit` to turn by (`AngleUnit.Radians` or `AngleUnit.Degrees`)

```java
TurnCommand(Follower follower, double angle, boolean isLeft, AngleUnit angleUnit)

// Example
new TurnCommand(follower, 90.0, true, AngleUnit.DEGREES)
```

To see how you can use this command in a [`CommandOpMode`](/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java).


# TurnToCommand

package com.seattlesolvers.solverslib.pedropathing

This command calls Pedro Pathing's [follower.turnTo(radians)](https://github.com/Pedro-Pathing/PedroPathing/blob/main/core/src/main/java/com/pedropathing/follower/Follower.java#L675), which allows you to easily turn directly to a certain amount of radians (or degrees) in place.

It has three parameters, with the first two being mandatory:

1. Pedro Pathing's Follower (which controls the robot movement)
2. The angle to turn by (default is Radians)

```java
TurnToCommand(Follower follower, double angle)

// Example
new TurnToCommand(follower, Math.PI / 2)
```

3. An optional parameter for a custom `AngleUnit` to turn to (`AngleUnit.Radians` or `AngleUnit.Degrees`)

```java
TurnToCommand(Follower follower, double angle, AngleUnit angleUnit)

// Example
new TurnToCommand(follower, 90.0, AngleUnit.DEGREES)
```

To see how you can use this command in a [`CommandOpMode`](/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java).


# What is Photon?

package com.seattlesolvers.solverslib.photon

Photon parallelizes hardware writes for servos and motors to make your robot's loop times faster. This is a stable version of the [now-archived PhotonCore](https://github.com/Eeshwar-Krishnan/PhotonFTC), that doesn't have probems.

{% hint style="warning" %}
Photon should be a last resort for improving loop times. Other traditional ways to help improve loop times can be found at <https://cookbook.dairy.foundation/improving_loop_times/improving_loop_times.html>.
{% endhint %}

#### How it Works

Normally with the SDK, after a setPower() or other LynxCommand, it waits for it to finish and check that it happened. But with Photon, after a setPower() or other LynxCommand, it immediately moves onto the next line of code/hardware write.

It is most effective with Swerve, because of the high amount of hardware writes frequently happening (4 servos + 4 motors).

#### Installation

See [installation page](https://docs.seattlesolvers.com/photon/pages/pfxSCrt0ymNmrj0kRXOE#build.gradle).

#### Usage

See [usage page](/photon/usage).


# Usage

package com.seattlesolvers.solverslib.photon

Photon only has 1 main requirement: you must use USB to connect hubs (USB 3.0 on Control Hub to USB mini b on expansion hub), NOT RS485. This is because Photon only works on USB-connected hubs.

{% hint style="danger" %}
**Warning:** Using USB instead of RS485 to communicate between hubs is not optional. You must use it for stable results, even if you are only running Photon on one hub.parallelized
{% endhint %}

{% hint style="info" %}
If you have a camera and wish to use Photon, it is recommended to use a USB 3.0 Hub to ensure that both the Expansion Hub and the camera is connected to the USB 3.0 port.&#x20;
{% endhint %}

To use in code with manual bulk caching:

```java
// Setup in your Robot class if you have one, or in init at start of opMode
// Don't do manual or auto bulk caching elsewhere - do it here.
PhotonCore.CONTROL_HUB.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
PhotonCore.EXPANSION_HUB.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
PhotonCore.experimental.setMaximumParallelCommands(8); // Can be adjusted based on user preference - but raising this number further can cause issues
PhotonCore.enable();
```

By default, both servos and motors will be parallelized. However, because Photon only works on USB-connected hubs, parallelizing servos will cause issues if you are powering servos using the REV Servo Hub, Servo Power Modules, or other non-USB devices. As such, you should disable `PARALLELIZE_SERVOS` when using non-USB devices to power servos.

```java
PhotonCore.PARALLELIZE_SERVOS = true; // Default set to true
PhotonCore.PARALLELIZE_SERVOS = false; // Set to false if using REV Servo Hub
```

{% hint style="info" %}
Using the goBILDA Servo Power Injector allows for full servo power (ideal for Swerve) while being Photon-compatible. This is because it directly uses the hub, and isn't a separate device.
{% endhint %}

Then, in every single run loop, you must clear the hub caches at the start (or end) of the loop to avoid getting stale values and get new data. This is the same for manual bulk reads as well.

```java
PhotonCore.CONTROL_HUB.clearBulkCache();
PhotonCore.EXPANSION_HUB.clearBulkCache();
```


# Odometry

package com.seattlesolvers.solverslib.kinematics

SolversLib offers its own odometry classes for the use of differential and holonomic drives. The odometry classes track the robot position as a `Pose2d`, which means it is represented using the vector $$\begin{pmatrix} x\ y\ \theta \end{pmatrix}$$ . $$x$$ is the distance in the forward direction of the robot, $$y$$ is the horizontal distance, and $$\theta$$ is the heading of the robot.

When using these classes, it is important to keep angles in radians. Distances should be consistent.

## Pose Exponential

Pose Exponential is a general FRC term for the constant velocity method of odometry utilized in FTC. We use pose exponentials because the cycle times of control loops in FTC can vary significantly.

This method uses differential equations to solve the nonlinear position of the robot given constant curvature. As such, we will not explain the math here. If you are interested in the math behind it, we suggest you read up on [Tyler's book](https://tavsys.net/controls-in-frc).

### Offsets and Trackwidth

The trackwidth is the distance between parallel encoders. This value should be tuned so that a precise calculation can be made. This value is a required pass into the constructor.

The center wheel offset accounts for the distance between the center of rotation of the robot and the position of the horizontal encoder. This is only necessary for the holonomic odometry.

To tune these values, make a rough estimate with a measured value and then use some sort of method for testing and updating the values until the outputs become reliable. Alternatively to beginning with a measured value, you can start at 18. Your value should then converge to some smaller distance.

## Creating the Odometry

A sample usage of SolversLib odometry can be found in this [sample folder](https://github.com/FTC-23511/SolversLib/tree/master/examples/src/main/java/org/firstinspires/ftc/teamcode/SharedOdometry). A sample for dead wheels can be found [here](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java).

### Using the Odometry Class

To use the odometry class, there are three different constructors depending on how you want to run your odometry. One method of running the odometry is using suppliers to have the class update your positions for you. The other is to manually input the sensor data into the `update()` method, which has parameters of the left encoder value, right encoder value, and horizontal encoder value (which is only used for holonomic). If you use suppliers, you can just call the `updatePose()` method which uses the suppliers to call the `update()` method.

```java
// define our trackwidth
static final double TRACKWIDTH = 13.7;

// convert ticks to inches
static final double TICKS_TO_INCHES = 15.3;

// create our encoders
MotorEx encoderLeft, encoderRight;
encoderLeft = new MotorEx(hardwareMap, "left_encoder");
encoderRight = new MotorEx(hardwareMap, "right_encoder");

// create our odometry
DifferentialOdometry diffOdom = new DifferentialOdometry(
    () -> encoderLeft.getCurrentPosition() * TICKS_TO_INCHES,
    () -> encoderRight.getCurrentPosition() * TICKS_TO_INCHES,
    TRACKWIDTH
);

// update the initial position
diffOdom.updatePose(new Pose2d(1, 2, 0));

// control loop
while (!isStopRequested()) {
    /* implementation */

    // update the position
    diffOdom.updatePose();
}
```

You should call the respective update method once every cycle of the control loop.

### Using the Odometry Subsystem

The [OdometrySubsystem](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/OdometrySubsystem.java) class is a template subsystem meant to make command-based programming around odometry much simpler and functional. Using the odometry subsystem makes it more accurate because the position will update itself through the scheduler's call to its `periodic()` method, which updates the position. The subsystem makes use of the suppliers, so you will **need** to use that constructor instead of the other for the SolversLib subsystem. Alternatively, you can create your own odometry subsystem.

```java
// define our constants
static final double TRACKWIDTH = 13.7;
static final double TICKS_TO_INCHES = 15.3;
static final double CENTER_WHEEL_OFFSET = 2.4;

// create our encoders
MotorEx encoderLeft, encoderRight, encoderPerp;
encoderLeft = new MotorEx(hardwareMap, "left_encoder");
encoderRight = new MotorEx(hardwareMap, "right_encoder");
encoderPerp = new MotorEx(hardwareMap, "center_encoder");

encoderLeft.setDistancePerPulse(TICKS_TO_INCHES);
encoderRight.setDistancePerPulse(TICKS_TO_INCHES);
encoderPerp.setDistancePerPulse(TICKS_TO_INCHES);

// create the odometry object
HolonomicOdometry holOdom = new HolonomicOdometry(
    encoderLeft::getDistance,
    encoderRight::getDistance,
    encoderPerp::getDistance,
    TRACKWIDTH, CENTER_WHEEL_OFFSET
);

// create the odometry subsystem
OdometrySubsystem odometry = new OdometrySubsystem(holOdom);
```

The [PurePursuitCommand](https://docs.seattlesolvers.com/pathing/pure-pursuit#using-the-pure-pursuit-command) makes use of the OdometrySubsystem class.


# WPILib Kinematics

package com.seattlesolvers.solverslib.kinematics.wpilibkinematics

Check out the [WPILib documentation](https://docs.wpilib.org/en/latest/docs/software/kinematics-and-odometry/index.html).

## What are Kinematics?

The kinematics suite contains classes for differential drive, swerve drive, and mecanum drive kinematics and odometry. The kinematics classes help convert between a universal `ChassisSpeeds` object, containing linear and angular velocities for a robot to usable speeds for each individual type of [drivetrain](/features/drivebases) i.e. left and right wheel speeds for a differential drive, four wheel speeds for a mecanum drive, or individual module states (speed and angle) for a swerve drive.

## What is Odometry?

Odometry involves using sensors on the robot to create an estimate of the position of the robot on the field. In FTC, these sensors are typically several encoders (the exact number depends on the drive type) and an optional gyroscope to measure robot angle. The odometry classes utilize the kinematics classes along with periodic user inputs about speeds (and angles in the case of swerve) to create an estimate of the robot’s location on the field. This odometry is a means of working around the odometer system that is often used by FTC teams.

## The `ChassisSpeeds` Class

The `ChassisSpeeds` object is essential to the WPILib kinematics and odometry suite. The `ChassisSpeeds` object represents the speeds of a robot chassis. This struct has three components:

* `vx`: The velocity of the robot in the x (forward) direction.
* `vy`: The velocity of the robot in the y (sideways) direction. (Positive values mean the robot is moving to the left).
* `omega`: The angular velocity of the robot in radians per second.

A non-holonomic drivetrain (i.e. a drivetrain that cannot move sideways, ex: a differential drive) will have a `vy` component of zero because of its inability to move sideways.

### Constructing a ChassisSpeeds Object

The constructor for the `ChassisSpeeds` object is very straightforward, accepting three arguments for `vx`, `vy`, and `omega`. In Java, `vx` and `vy` must be in meters per second. In C++, the units library may be used to provide a linear velocity using any linear velocity unit.

```java
// The robot is moving at 3 meters per second forward, 2 meters
// per second to the right, and rotating at half a rotation per
// second counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(3.0, -2.0, Math.PI);
```

### Creating a ChassisSpeeds Object from Field-Relative Speeds

A `ChassisSpeeds` object can also be created from a set of field-relative speeds when the robot angle is given. This converts a set of desired velocities relative to the field (for example, toward the opposite alliance station and toward the right field boundary) to a `ChassisSpeeds` object which represents speeds that are relative to the robot frame. This is useful for implementing field-oriented controls for a swerve or mecanum drive robot.

The static `ChassisSpeeds.fromFieldRelativeSpeeds()` method can be used to generate the `ChassisSpeeds` object from field-relative speeds. This method accepts the `vx` (relative to the field), `vy` (relative to the field), `omega`, and the robot angle.

```java
// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise. The current
// robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds
(
    2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);
```

The angular velocity is not explicitly stated to be “relative to the field” because the angular velocity is the same as measured from a field perspective or a robot perspective.


# Old Kinematics

Swerve Drive Kinematics have been rewritten, and hence, the old Kinematics classes have been Deprecated. A reference for these older classes can be found in the subpages of this page.


# Swerve Drive Odometry

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.SwerveDriveOdometry

A user can use the swerve drive kinematics classes in order to perform [odometry](/kinematics/wpilib-kinematics#what-is-odometry). WPILib/SolversLib contains a `SwerveDriveOdometry` class that can be used to track the position of a swerve drive robot on the field.

**Note**:\
Because this method only uses encoders and a gyro, the estimate of the robot’s position on the field will drift over time, especially as your robot comes into contact with other robots during gameplay. However, odometry is usually very accurate during the autonomous period.

## Creating the Odometry Object

The `SwerveDriveOdometry` class requires two mandatory arguments, and one optional argument. The mandatory arguments are the kinematics object that represents your swerve drive (in the form of a `SwerveDriveKinematics` class) and the angle reported by your gyroscope (as a Rotation2d). The third optional argument is the starting pose of your robot on the field (as a `Pose2d`). By default, the robot will start at $$\begin{pmatrix} x\ y\ \theta \end{pmatrix} = \begin{pmatrix} 0\ 0\ 0 \end{pmatrix}$$ .

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase.

```java
// Locations for the swerve drive modules relative to the
// robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);

// Creating my odometry object from the kinematics object. Here,
// our starting pose is 5 meters along the long end of the
// field and in the
// center of the field along the short end, facing forward.
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry
(
  m_kinematics, getGyroHeading(),
  new Pose2d(5.0, 13.5, new Rotation2d()
);
```

## Updating the Robot Pose

The `update` method of the odometry class updates the robot position on the field. The update method takes in the gyro angle of the robot, along with a series of module states (speeds and angles) in the form of a `SwerveModuleState` each. It is important that the order in which you pass the `SwerveModuleState` objects is the same as the order in which you created the kinematics object.

This `update` method must be called periodically, preferably in the `periodic()` method of a [Subsystem](/command-base/command-system/subsystems). The `update` method returns the new updated pose of the robot.

```java
@Override
public void periodic() {
  // Get my gyro angle.
  Rotation2d gyroAngle = Rotation2d.fromDegrees(m_gyro.getHeading());

  // Update the pose
  m_pose = m_odometry.update
  (
    gyroAngle, m_frontLeftModule.getState(),
    m_frontRightModule.getState(),
    m_backLeftModule.getState(), m_backRightModule.getState()
  );
}
```

### Resetting the Robot Pose

The robot pose can be reset via the `resetPose` method. This method accepts two arguments – the new field-relative pose and the current gyro angle.

If at any time, you decide to reset your gyroscope, the `resetPose` method MUST be called with the new gyro angle.

The implementation of `getState()` above is left to the user. The idea is to get the module state (speed and angle) from each module.

In addition, the `getPoseMeters()` method can be used to retrieve the current robot pose without an update.


# Swerve Drive Kinematics

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.SwerveDriveKinematics

The `SwerveDriveKinematics` class is a useful tool that converts between a `ChassisSpeeds` object and several `SwerveModuleState` objects, which contains velocities and angles for each swerve module of a swerve drive robot.

## The `SwerveModuleState` Class

The `SwerveModuleState` class contains information about the velocity and angle of a singular module of a swerve drive. The constructor for a `SwerveModuleState` takes in two arguments, the velocity of the wheel on the module, and the angle of the module.

The velocity of the wheel must be in meters per second. An angle of 0 from the module represents the forward-facing direction.

## Constructing the Kinematics Object

The `SwerveDriveKinematics` class accepts a variable number of constructor arguments, with each argument being the location of a swerve module relative to the robot center (as a `Translation2d`. The number of constructor arguments corresponds to the number of swerve modules. A swerve bot must have AT LEAST two swerve modules.

The locations for the modules must be relative to the center of the robot. Positive x values represent moving toward the front of the robot whereas positive y values represent moving toward the left of the robot.

```java
// Locations for the swerve drive modules
// relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);
```

## Converting Chassis Speeds to Module States

The `toSwerveModuleStates(ChassisSpeeds speeds)` method should be used to convert a `ChassisSpeeds` object to a an array of `SwerveModuleState` objects. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual module states.

The elements in the array that is returned by this method are the same order in which the kinematics object was constructed. For example, if the kinematics object was constructed with the front left module location, front right module location, back left module location, and the back right module location in that order, the elements in the array would be the front left module state, front right module state, back left module state, and back right module state in that order.

```java
// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to module states
SwerveModuleState[] moduleStates =
    kinematics.toSwerveModuleStates(speeds);

// Front left module state
SwerveModuleState frontLeft = moduleStates[0];

// Front right module state
SwerveModuleState frontRight = moduleStates[1];

// Back left module state
SwerveModuleState backLeft = moduleStates[2];

// Back right module state
SwerveModuleState backRight = moduleStates[3];
```

### Field-Oriented Drive

[Recall](/kinematics/wpilib-kinematics) that a `ChassisSpeeds` object can be created from a set of desired field-oriented speeds. This feature can be used to get module states from a set of desired field-oriented speeds.

```java
// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise.
// The current robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);

// Now use this in our kinematics
SwerveModuleState[] moduleStates =
  kinematics.toSwerveModuleStates(speeds);
```

### Using Custom Centers of Rotation

Sometimes, rotating around one specific corner might be desirable for certain evasive maneuvers. This type of behavior is also supported by the WPILib classes. The same `ToSwerveModuleStates()` method accepts a second parameter for the center of rotation (as a `Translation2d`). Just like the wheel locations, the `Translation2d` representing the center of rotation should be relative to the robot center.

Because all robots are a rigid frame, the provided `vx` and `vy` velocities from the `ChassisSpeeds` object will still apply for the entirety of the robot. However, the `omega` from the `ChassisSpeeds` object will be measured from the center of rotation.

For example, one can set the center of rotation on a certain module and if the provided `ChassisSpeeds` object has a `vx` and `vy` of zero and a non-zero `omega`, the robot will appear to rotate around that particular swerve module.

## Converting Module States to Chassis Speeds

One can also use the kinematics object to convert an array of `SwerveModuleState` objects to a singular `ChassisSpeeds` object. The `toChassisSpeeds(SwerveModuleState... states)` method can be used to achieve this.

```java
// Example module states
SwerveModuleState frontLeftState =
  new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleState frontRightState =
  new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleState backLeftState =
  new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleState backRightState =
  new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(
  frontLeftState, frontRightState, backLeftState, backRightState
);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;
```


# Differential Drive Kinematics

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.DifferentialDriveKinematics

The `DifferentialDriveKinematics` class is a useful tool that converts between a `ChassisSpeeds` object and a `DifferentialDriveWheelSpeeds` object, which contains velocities for the left and right sides of a differential drive robot.

## Constructing the Kinematics Object

The `DifferentialDriveKinematics` object accepts one constructor argument, which is the trackwidth of the robot. This represents the distance between the two sets of wheels on a differential drive.

**Note**: The trackwidth must be in meters.

## Converting Chassis Speeds to Wheel Speeds

The `toWheelSpeeds(ChassisSpeeds speeds)` method should be used to convert a `ChassisSpeeds` object to a `DifferentialDriveWheelSpeeds` object. This is useful in situations where you have to convert a linear velocity (`vx`) and an angular velocity (`omega`) to left and right wheel velocities.

```java
// Creating my kinematics object: track width of 15 inches
DifferentialDriveKinematics kinematics =
  new DifferentialDriveKinematics(15.0 / 254.0);

// Example chassis speeds: 2 meters per second linear velocity,
// 1 radian per second angular velocity.
ChassisSpeeds chassisSpeeds = new ChassisSpeeds(2.0, 0, 1.0);

// Convert to wheel speeds
DifferentialDriveWheelSpeeds wheelSpeeds =
  kinematics.toWheelSpeeds(chassisSpeeds);

// Left velocity
double leftVelocity = wheelSpeeds.leftMetersPerSecond;

// Right velocity
double rightVelocity = wheelSpeeds.rightMetersPerSecond;
```

## Converting Wheel Speeds to Chassis Speeds

One can also use the kinematics object to convert individual wheel speeds (left and right) to a singular `ChassisSpeeds` object. The `toChassisSpeeds(DifferentialDriveWheelSpeeds speeds)` method should be used to achieve this.

```java
// Creating my kinematics object: track width of 15 inches
DifferentialDriveKinematics kinematics =
  new DifferentialDriveKinematics(15.0 / 254.0);

// Example differential drive wheel speeds: 2 meters per second
// for the left side, 3 meters per second for the right side.
DifferentialDriveWheelSpeeds wheelSpeeds =
  new DifferentialDriveWheelSpeeds(2.0, 3.0);

// Convert to chassis speeds.
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds);

// Linear velocity
double linearVelocity = chassisSpeeds.vxMetersPerSecond;

// Angular velocity
double angularVelocity = chassisSpeeds.omegaRadiansPerSecond;
```


# Differential Drive Odometry

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.DifferentialDriveOdometry

A user can use the differential drive kinematics classes in order to perform [odometry](/kinematics/wpilib-kinematics#what-is-odometry). WPILib/SolversLib contains a `DifferentialDriveOdometry` class that can be used to track the position of a differential drive robot on the field.

**Note**:\
Because this method only uses encoders and a gyro, the estimate of the robot’s position on the field will drift over time, especially as your robot comes into contact with other robots during gameplay. However, odometry is usually very accurate during the autonomous period.

## Creating the Odometry Object

The `DifferentialDriveOdometry` class requires one mandatory argument and one optional argument. The mandatory argument is the angle reported by your gyroscope (as a Rotation2d). The optional argument is the starting pose of your robot on the field (as a `Pose2d`). By default, the robot will start at $$\begin{pmatrix} x\ y\ \theta \end{pmatrix} = \begin{pmatrix} 0\ 0\ 0 \end{pmatrix}$$ .

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase.

The encoder positions **must** be reset to zero before constructing the `DifferentialDriveOdometry` class.

```java
// Creating my odometry object. Here,
// our starting pose is 5 meters along the long end of the field
// and in the center of the field along the short end,
// facing forward.
DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry
(
    getGyroHeading(), new Pose2d(5.0, 13.5, new Rotation2d()
);
```

## Updating the Robot Pose

The `update` method can be used to update the robot’s position on the field. This method must be called periodically, preferably in the `periodic()` method of a [Subsystem](/command-base/command-system/subsystems). The `update` method returns the new updated pose of the robot. This method takes in the gyro angle of the robot, along with the left encoder distance and right encoder distance.

Ensure your encoder distances are in meters!

### Resetting the Robot Pose

The robot pose can be reset via the `resetPose` method. This method accepts two arguments – the new field-relative pose and the current gyro angle.

If at any time, you decide to reset your gyroscope, the `resetPose` method MUST be called with the new gyro angle. Furthermore, the encoders must also be reset to zero when resetting the pose.


# Mecanum Drive Kinematics

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.MecanumDriveKinematics

The `MecanumDriveKinematics` class is a useful tool that converts between a `ChassisSpeeds` object and a `MecanumDriveWheelSpeeds` object, which contains velocities for each of the four wheels on a mecanum drive.

## Constructing the Kinematics Object

The `MecanumDriveKinematics` class accepts four constructor arguments, with each argument being the location of a wheel relative to the robot center (as a `Translation2d`). The order for the arguments is front left, front right, back left, and back right. The locations for the wheels must be relative to the center of the robot. Positive x values represent moving toward the front of the robot whereas positive y values represent moving toward the left of the robot.

```java
// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);
```

## Converting Chassis Speeds to Wheel Speeds

The `toWheelSpeeds(ChassisSpeeds speeds)` method should be used to convert a `ChassisSpeeds` object to a `MecanumDriveWheelSpeeds` object. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual wheel speeds.

```java
// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds =
    kinematics.toWheelSpeeds(speeds);

// Get the individual wheel speeds
double frontLeft = wheelSpeeds.frontLeftMetersPerSecond;
double frontRight = wheelSpeeds.frontRightMetersPerSecond;
double backLeft = wheelSpeeds.rearLeftMetersPerSecond;
double backRight = wheelSpeeds.rearRightMetersPerSecond;
```

### Field-Oriented Drive

[Recall](/kinematics/wpilib-kinematics) that a `ChassisSpeeds` object can be created from a set of desired field-oriented speeds. This feature can be used to get wheel speeds from a set of desired field-oriented speeds.

```java
// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise.
// The current robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);

// Now use this in our kinematics
MecanumDriveWheelSpeeds wheelSpeeds =
  kinematics.toWheelSpeeds(speeds);
```

### Using Custom Centers of Rotation

Sometimes, rotating around one specific corner might be desirable for certain evasive maneuvers. This type of behavior is also supported by the WPILib classes. The same `toWheelSpeeds()` method accepts a second parameter for the center of rotation (as a `Translation2d`). Just like the wheel locations, the `Translation2d` representing the center of rotation should be relative to the robot center.

Because all robots are a rigid frame, the provided `vx` and `vy` velocities from the `ChassisSpeeds` object will still apply for the entirety of the robot. However, the `omega` from the `ChassisSpeeds` object will be measured from the center of rotation.

For example, one can set the center of rotation on a certain wheel and if the provided `ChassisSpeeds` object has a `vx` and `vy` of zero and a non-zero `omega`, the robot will appear to rotate around that particular wheel.

## Converting Wheel Speeds to Chassis speeds

One can also use the kinematics object to convert a `MecanumDriveWheelSpeeds` object to a singular `ChassisSpeeds` object. The `toChassisSpeeds(MecanumDriveWheelSpeeds speeds)` method can be used to achieve this.

```java
// Example wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds =
    new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds =
    kinematics.toChassisSpeeds(wheelSpeeds);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;
```


# Mecanum Drive Odometry

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.MecanumDriveOdometry

A user can use the mecanum drive kinematics classes in order to perform [odometry](/kinematics/wpilib-kinematics#what-is-odometry). WPILib/SolversLib contains a `MecanumDriveOdometry` class that can be used to track the position of a mecanum drive robot on the field.

**Note**:\
Because this method only uses encoders and a gyro, the estimate of the robot’s position on the field will drift over time, especially as your robot comes into contact with other robots during gameplay. However, odometry is usually very accurate during the autonomous period.

## Creating the Odometry Object

The `MecanumDriveOdometry` class requires two mandatory arguments and one optional argument. The mandatory arguments are the kinematics object that represents your mecanum drive (in the form of a `MecanumDriveKinematics` class) and the angle reported by your gyroscope (as a Rotation2d). The third optional argument is the starting pose of your robot on the field (as a `Pose2d`). By default, the robot will start at $$\begin{pmatrix} x\ y\ \theta \end{pmatrix} = \begin{pmatrix} 0\ 0\ 0 \end{pmatrix}$$ .

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase.

```java
// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);

// Creating my odometry object from the kinematics object. Here,
// our starting pose is 5 meters along the long end of the field and in the
// center of the field along the short end, facing forward.
MecanumDriveOdometry m_odometry = new MecanumDriveOdometry
(
  m_kinematics, getGyroHeading(),
  new Pose2d(5.0, 13.5, new Rotation2d()
);
```

## Updating the Robot Pose

The `update` method of the odometry class updates the robot position on the field. The update method takes in the gyro angle of the robot, along with a `MecanumDriveWheelSpeeds` object representing the speed of each of the 4 wheels on the robot. This `update` method must be called periodically, preferably in the `periodic()` method of a [Subsystem](/command-base/command-system/subsystems). The `update` method returns the new updated pose of the robot.

The `MecanumDriveWheelSpeeds` class must be constructed with each wheel speed in meters per second.

```java
@Override
public void periodic() {
  // Get my wheel speeds; assume .getRate() has been
  // set up to return velocity of the encoder
  // in meters per second.
  MecanumDriveWheelSpeeds wheelSpeeds = new MecanumDriveWheelSpeeds
  (
      m_frontLeftEncoder.getRate(), m_frontRightEncoder.getRate(),
      m_backLeftEncoder.getRate(), m_backRightEncoder.getRate()
  );

  // Get my gyro angle.
  Rotation2d gyroAngle = Rotation2d.fromDegrees(m_gyro.getAngle());

  // Update the pose
  m_pose = m_odometry.update(gyroAngle, wheelSpeeds);
}
```

### Resetting the Robot Pose

The robot pose can be reset via the `resetPose` method. This method accepts two arguments – the new field-relative pose and the current gyro angle.

If at any time, you decide to reset your gyroscope, the `resetPose` method MUST be called with the new gyro angle.

In addition, the `getPoseMeters()` method can be used to retrieve the current robot pose without an update.


# Point-to-Point

package com.seattlesolvers.solverslib.p2p

SolversLib Point-to-Point (p2p) for Swerve is still a WIP, and full documentation is coming soon!

The full source code is in SolversLib's p2p directory, and can be viewed [here](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/p2p).

You can check out the beta visualizer for SolversLib p2p at <https://visualizer.seattlesolvers.com/>.


# Pure Pursuit

package com.seattlesolvers.solverslib.purepursuit

## SolversLib Pure Pursuit is buggy, and has been replaced with [Point-to-Point](/pathing/point-to-point). Its use is currently not recommend.

## Pure Pursuit

The pure pursuit algorithm in SolversLib is developed so that the user only needs to add the desired waypoints and call the `followPath()` method in the [Path](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/purepursuit/Path.java) class. To use this, you need to pass the [mecanum](https://docs.SolversLib.org/SolversLib/features/drivebases#mecanum) drivetrain as well as the odometry for the robot. Once the method is finished, it will return true or false depending on if it was successful or not.

As an alternative, you can call the `loop()` method and directly input your odometry positions there. Make sure you update the odometry positions with each iteration of the loop.

### What is Pure Pursuit?

Pure pursuit, otherwise designated as "PP," is a path tracking algorithm that calculates the robot velocity in order to reach a designated look-ahead point from the current position. It loosely follows a path determined by a set of waypoints, which are coordinates on the field. What the pure pursuit controller does is create a circle of determined radius and follow the path by "looking ahead" with the circle and seeing where it intersects with the path. The robot's heading orientation is then compared to the radius that connects the center of the robot to that intersection. It then moves in correspondence. The radius size can be updated for each waypoint you enter into the path for specificity.

![A visual representation of look-ahead](/files/ompp5AF5xM4zG3U2r2eg)

The robot continues to follow this intersection at real-time. This is how the robot "follows" the designated path. It is essentially a p controller for the heading that has the robot move at the fastest possible speed around some path.

#### Waypoints

There are five types of waypoints: start, general, interrupted, point-turn, and end. The starting waypoint represents the first point in the path; conversely, the ending waypoint is the last point in the path.

A general waypoint is a point where the robot performs its ordinary pursuit algorithm with the look-ahead method. A point-turn waypoint is a type of general waypoint that stops at the given point, turns, and then traverses to the next waypoint. An interrupted waypoint is a type of point-turn waypoint where other actions can occur, such as picking up a skystone.

**Creating Waypoints**

You can create a waypoint by calling the various constructors.

**StartWaypoint**

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p1 = new StartWaypoint();

// With Pose2d.
Waypoint p1 = new StartWaypoint(pose2d);
// With X and Y coordinates.
Waypoint p1 = new StartWaypoint(x, y);
// With Translation2d.
Waypoint p1 = new StartWaypoint(translation2d);
```

**GeneralWaypoint**

Each general waypoint inherits from the previous general waypoint in the path. The only parameters that will need to be specified are the x and y coordinates of the point. Each point has the option to update the different parameters across the path (which is meant for user-end customization of the path). Point-turn waypoints, interrupted waypoints, and end waypoints are all subclasses of a general waypoint, so they will also have this feature. This inheritance is performed in the `init()` method of the path.

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p2 = new GeneralWaypoint();

// With X and Y coordinates. This waypoint will inherit
// its settings from the previous waypoint.
// Useful for long strings of waypoints.
// Note: Will not work if the waypoint preceding
// this is not an instance of GeneralWaypoint. 
Waypoint p2 = new GeneralWaypoint(x, y);

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p2 = new GeneralWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius
);
// With Pose2D.
Waypoint p2 = new GeneralWaypoint(
    pose2d, movementSpeed, turnSpeed,
    followRadius
);
// With X and Y coordinates.
Waypoint p2 = new GeneralWaypoint(
    x, y, movementSpeed,
    turnSpeed, followRadius
);
// With a preferred angle.
Waypoint p2 = new GeneralWaypoint(
    x, y, rotationRadians,    
    movementSpeed, turnSpeed,
    followRadius
);
```

**PointTurnWaypoint**

As you will see here, a "buffer" is a sort of expected error. This sets up a tolerance given that the robot might be a bit offset from the desired position or rotation.

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p3 = new PointTurnWaypoint();

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p3 = new PointTurnWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer
);
// With Pose2D.
Waypoint p3 = new PointTurnWaypoint(
    pose2d, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
// With X and Y coordinates and preferred angle.
Waypoint p3 = new PointTurnWaypoint(
    x, y, rotationRadians, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
// With X and Y coordinates.
Waypoint p3 = new PointTurnWaypoint(
    x, y, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
```

**InterruptWaypoint**

The `action` here is an [InterruptAction](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/purepursuit/actions/InterruptAction.java), which is an interface that the user can implement to create a custom action to occur at this point. A recommendation is to pair this with the [command paradigm](/command-base/command-system) that SolversLib provides.

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p4 = new InterruptWaypoint();

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p4 = new InterruptWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, action
);
// With Pose2D.
Waypoint p4 = new InterruptWaypoint(
    pose2d, movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, action
);
// With X and Y coordinates and preferred angle.
Waypoint p4 = new InterruptWaypoint(
    x, y, rotationRadians,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, action
);
// With X and Y coordinates.
Waypoint p4 = new InterruptWaypoint(
    x, y, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer,
    action
);

// With java 8 you can use a lambda expression to easily
// add an action. For example:
Waypoint p4 = new InterruptWaypoint(
    x, y, movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, grabSubsystem::grabBlock
);
```

**EndWaypoint**

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p5 = new EndWaypoint();

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p5 = new PointTurnWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer
);
// With Pose2D.
Waypoint p5 = new PointTurnWaypoint(
    pose2d, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
// With X and Y coordinates and preferred angle
// (A preferred angle is needed for an EndWaypoint).
Waypoint p5 = new PointTurnWaypoint(
    x, y, rotationRadians, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
```

### Creating the Path

Before you can call the `loop()` or `followPath()` method, you need to follow the proper procedure. If you are using `loop()`, you need to call the `init()` method to ensure your path is legal and set up the unconfigured waypoints.

```java
// we are using the waypoints we made in the above examples
Path m_path = new Path(p1, p2, p3, p4, p5);

m_path.init(); // initialize the path
```

If the path is not legal, an exception will be thrown.

#### Intersections

An intersection is the point where the follow distance represented by a circle around the robot meets the drawn path derived from the waypoints. The "best intersection" is determined by either waypoint order or heading. This intersection point where the circle meets the path is where the robot will move to. The pure pursuit algorithm determines the best intersection and calculates the motor powers needed to reach said position. This updates with each loop, so the intersection point can change with each step due to the movement of the robot. While the conventional pure pursuit algorithm used heading controlled waypoints, SolversLib features a custom type of intersection control we call "order controlled". This type of control is more powerful and less prone to errors then heading controlled and is enabled by default. If you wish to use heading controlled instead, use this (not recommended):

```java
m_path.setPathType(PathType.HEADING_CONTROLLED);
```

#### Retrace

SolversLib's pure pursuit implementation includes a unique feature we call retrace. One common issue with pure pursuit is that the robot can lose it's path. Retrace solves this issue. If enabled (retrace is enabled by default) and the robot loses it's path, the software will automatically plot a temporary path back to it's last known path position. Once the robot finds the path again it will continue on as normal. If you wish to disable retrace (not recommended), do this:

```java
m_path.disableRetrace();
```

#### Timeouts

Advanced teams may want to have more control over how long the robot get to have to complete a path. If the robot is stuck on a path/waypoint for too long, you may want to stop the path to avoid accidental penalties. To set timeouts do the following:

```java
// For an entire path
m_path.setWaypointTimeouts(timeout);

// For individual waypoints
m_path.setWaypointTimeouts(p1_timeout, p2_timeout, p3_timeout, ...);

// Reset timeouts.
m_path.resetTimeouts();
```

#### Resetting the Path

If you want to use a path more than once in the same opmode, make sure to reset between uses. You can do this as follows:

```java
m_path.reset();
```

### Using `followPath()`

The `followPath()` method is the automatic implementation of pure pursuit for SolversLib. For teams that want to use all of SolversLib's features to the fullest, this is the recommended process.

An important note for the pure pursuit algorithm is that it only works well with odometry. You can use the various odometry systems provided by SolversLib. An important thing to note is that `followPath()` makes use of the [Odometry](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/kinematics/Odometry.java) abstract class and the [mecanum drivebase](https://docs.SolversLib.org/SolversLib/features/drivebases#mecanum). Then, the method will call the loop method and do everything for you.

```java
// follow path
m_path.followPath(m_robotDrive, m_robotOdometry);
```

An issue this method has is that we cannot directly access the hardware of the robot. As a result, the `update()` method for the odometry is not called in `followPath()`. Instead, we call `updatePose()`.

#### Creating Your Odometry

As a way of working around this issue, the odometry needs to be setup in a particular way with [Suppliers](https://docs.oracle.com/javase/8/docs/api/java/util/function/Supplier.html). A supplier is a functional interface that uses lambdas to reference a certain value. Let's work with the [HolonomicOdometry](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/kinematics/HolonomicOdometry.java) class for these examples.

You're going to want to instantiate your odometry using this constructor:

```java
HolonomicOdometry m_robotOdometry = new HolonomicOdometry(
    leftValue, rightValue, horizontalValue, trackWidth,
    centerWheelOffset
);
```

**Creating the Suppliers**

Before we can create the object, we need to make our suppliers. We will do this by using the [DoubleSupplier](https://docs.oracle.com/javase/8/docs/api/java/util/function/DoubleSupplier.html) implementation. We need to create three of these objects: one for each odometer. Let's assume the user has created a method that automatically converts ticks to inches for an external encoder.

```java
DoubleSupplier leftValue, rightValue, horizontalValue;

leftValue = () -> ticksToInches(m_lOdom.getCurrentPosition());
rightValue = () -> ticksToInches(m_rOdom.getCurrentPosition());
horizontalValue = () -> ticksToInches(m_hOdom.getCurrentPosition());
```

### Using `loop()`

The alternative to the `followPath()` method is the `loop()` method. For teams that want to use solely the SolversLib implementation of pure pursuit and perform the rest of the actions themselves, then this is the more appealing method.

The use of suppliers can be avoided using this method since it can be called in your own class with access to the hardware directly.

#### Odometry Options

The odometry is much more open for this. You can use whatever constructor you desire for it. Since the method parameters only take x, y, and heading values, you can use whatever odometry system you desire as long as it produces such values. This is one of the more appealing aspects of the `loop()` method.

The important thing for odometry is to remember to update the position of the robot after each iteration after manually inputting the motor speeds.

#### Calling the Method

This is the principle path method. After everything is configured and initiated, this method can be used. Using the robot's x, y, and rotation, this method calculates the appropriate motor powers for the robot to follow the path. This method calls all triggered/interrupted actions automatically. If this returns zero motor speeds `{0, 0, 0}` that means the path has either (1) timed out, (2) lost the path and retrace was disabled, or (3) reached the destination. Use `isFinished()` and `timedOut()` to troubleshoot.

Below is an example using a custom robot class that includes the drivebase and odometry:

```java
while (!m_path.isFinished()) {
    if (m_path.timedOut())
        throw new InterruptedException("Timed out");

    // return the motor speeds
    double speeds[] = m_path.loop(m_robot.getX(), m_robot.getY(),
                                  m_robot.getHeading());

    m_robot.drive(speeds[0], speeds[1], speeds[2]);
    m_robot.updatePose();
}
m_robot.stop();
```

### Using the Pure Pursuit Command

If you're using your odometry for multiple subsystems, you're likely going to want to make use of the [PurePursuitCommand](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/command/PurePursuitCommand.java) due to the shared odometry (as we only want to update it once per cycle). This is actually the recommended method of using pure pursuit, especially if you want to use it with the command-based paradigm that SolversLib has to offer.

#### Creating an Odometry Subsystem

The pre-built PurePursuitCommand requires the use of SolversLib's [OdometrySubsystem](https://docs.SolversLib.org/SolversLib/kinematics/odometry#using-the-odometry-subsystem). It is fairly easy to set up. All that is needed is for the user to pass in their odometry class into the constructor of the subsystem.

```java
// create the odometry object
HolonomicOdometry m_robotOdometry = new HolonomicOdometry(
    leftValue, rightValue, horizontalValue, TRACKWIDTH,
    CENTER_WHEEL_OFFSET
);

// pass the odometry object into the subsystem constructor
OdometrySubsystem m_odometry = new OdometrySubsystem(
    m_robotOdometry
);
```

**Using the Odometry Subsystem**

Users can make use of the odometry subsystem in the exact same way as the Odometry class.

```java
// retrieve the current saved pose of the robot
Pose2d robotPose = m_odometry.getPose();

// update the pose manually
m_odometry.update();
```

The odometry subsystem updates the position of the robot in its `periodic()` method, so it will update every time the CommandScheduler is run. This effectively means that the position does not need to be updated manually unless desired by the user.

#### Creating a PurePursuitCommand

Creating the command is simple. Note that this implementation of the command does not utilize every feature of the Path and is relatively simplistic. It is designed to be a simple template for the user and not an end-all-be-all for every possible desired activity.

To create the object, pass in the drivebase object, the odometry subsystem, and the desired waypoints.

```java
PurePursuitCommand ppCommand = new PurePursuitCommand(
    m_robotDrive, m_odometry,
    p1, p2, p3, p4, p5
);

// schedule the command
ppCommand.schedule();

// remove waypoint at specified index
ppCommand.removeWayPointAtIndex(4);
ppCommand.removeWayPointAtIndex(3);
ppCommand.removeWayPointAtIndex(2);

// add waypoints to the path
ppCommand.addWaypoint(p3);
ppCommand.addWaypoints(p4, p5);
```

The rest of the class does everything for you through the command-based paradigm. Using the `execute()`, `end(interrupted)`, and `isFinished()` methods allows for the command to be run simply by running the scheduler in a loop.

#### Running the Command

It is run the exact same way everything else is run in the paradigm: by running the scheduler. Take a look at [this sample](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PurePursuitSample.java) to see how everything works together.


# Trajectory

package com.seattlesolvers.solverslib.trajectory

In FTC, there are often games that require an autonomous where robots are moving from one position to another—sometimes repeatedly. A lot of teams implement this motion by moving forward, turning, then moving forward again. Sometimes this is done with a time-base or a unit of known distance.

While these methods are functional, it is better if we can have the robot turn and drive at the same time to optimize the motion. Below is a video showing how this trajectory generation and following works:

{% embed url="<https://youtu.be/yVmJDOE3M2Y>" %}

You can find the same information in the [wpilib docs](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/trajectories/trajectory-generation.html).


# Trajectory Generation

This article goes over how to generate a trajectory. The next few articles in this series will go over how to actually follow the generated trajectory. There are a few things that your robot must have before you dive into the world of trajectories:

* A way to measure the position and velocity of each side of the robot. An encoder is the best way to do this; however, other options may include optical flow sensors, etc.
* A way to measure the angle or angular rate of the robot chassis. A gyroscope is the best way to do this. Although the angular rate can be calculated using encoder velocities, this method is NOT recommended because of wheel scrubbing.

## Splines

A [spline](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/spline) refers to a set of curves that interpolate between points. Think of it as connecting dots, except with curves. SolversLib supports two types of splines: hermite clamped cubic and hermite quintic.

* Hermite clamped cubic: This is the recommended option for most users. Generation of trajectories using these splines involves specifying the (x, y) coordinates of all points, and the headings at the start and end waypoints. The headings at the interior waypoints are automatically determined to ensure continuous curvature (rate of change of the heading) throughout.
* Hermite quintic: This is a more advanced option which requires the user to specify (x, y) coordinates and headings for *all* waypoints. This should be used if you are unhappy with the trajectories that are being generated by the clamped cubic splines or if you want finer control of headings at the interior points.

Splines are used as a tool to generate trajectories; however, the spline itself does not have any information about velocities and accelerations. Therefore, it is not recommended that you use the spline classes directly. In order to generate a smooth path with velocities and accelerations, a *trajectory* must be generated.

## Creating the Trajectory Configuration

A configuration must be created in order to generate a trajectory. The config contains information about special constraints, the max velocity, the max acceleration in addition to the start velocity and end velocity. The config also contains information about whether the trajectory should be reversed (robot travels backward along the waypoints). The `TrajectoryConfig` class should be used to construct a config. The constructor for this class takes two arguments, the max velocity and max acceleration. The other fields (`startVelocity`, `endVelocity`, `reversed`, `constraints`) are defaulted to reasonable values (`0`, `0`, `false`, `{}`) when the object is created. If you wish to modify the values of any of these fields, you can call the following methods:

* `setStartVelocity(double startVelocityMetersPerSecond)`
* `setEndVelocity(double endVelocityMetersPerSecond)`
* `setReversed(boolean reversed)`
* `addConstraint(TrajectoryConstraint constraint)`

### NOTE

The `reversed` property simply represents whether the robot is traveling backward. If you specify four waypoints, a, b, c, and d, the robot will still travel in the same order through the waypoints when the `reversed` flag is set to `true`. This also means that you must account for the direction of the robot when providing the waypoints. For example, if your robot is facing your alliance station wall and travels backwards to some field element, the starting waypoint should have a rotation of 180 degrees.

## Generating the Trajectory

The method used to generate a trajectory is `generateTrajectory(...)`. There are four overloads for this method. Two that use clamped cubic splines and the two others that use quintic splines. For each type of spline, there are two ways to construct a trajectory. The easiest methods are the overloads that accept `Pose2d` objects.

For clamped cubic splines, this method accepts two `Pose2d` objects, one for the starting waypoint and one for the ending waypoint. The method takes in a vector of `Translation2d` objects which represent the interior waypoints. The headings at these interior waypoints are determined automatically to ensure continuous curvature. For quintic splines, the method simply takes in a list of `Pose2d` objects, with each `Pose2d` representing a point and heading on the field.

The more complex overload accepts “control vectors” for splines. The `ControlVector` class consists of two `double` arrays. Each array represents one dimension (x or y), and its elements represent the derivatives at that point. For example, the value at element 0 of the `x` array represents the x coordinate (0th derivative), the value at element 1 represents the 1st derivative in the x dimension and so on.

When using clamped cubic splines, the length of the array must be 2 (0th and 1st derivatives), whereas when using quintic splines, the length of the array should be 3 (0th, 1st, and 2nd derivative). Unless you know exactly what you are doing, the first and simpler method is HIGHLY recommended for manually generating trajectories.

Here is an example of generating a trajectory using clamped cubic splines for the 2018 game, FIRST Power Up:

```java
class ExampleTrajectory {
  public void generateTrajectory() {

    // 2018 cross scale auto waypoints.
    var sideStart = new Pose2d(Units.feetToMeters(1.54), Units.feetToMeters(23.23),
        Rotation2d.fromDegrees(-180));
    var crossScale = new Pose2d(Units.feetToMeters(23.7), Units.feetToMeters(6.8),
        Rotation2d.fromDegrees(-160));

    var interiorWaypoints = new ArrayList<Translation2d>();
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(14.54), Units.feetToMeters(23.23)));
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(21.04), Units.feetToMeters(18.23)));

    TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(12), Units.feetToMeters(12));
    config.setReversed(true);

    var trajectory = TrajectoryGenerator.generateTrajectory(
        sideStart,
        interiorWaypoints,
        crossScale,
        config);
  }
}
```

Note that this is ***not*** code from SolversLib and is instead a sample from WPILib docs. The keyword `var` is used here, but that is a feature from Java 10 which is not supported for Android development. The keyword has the compiler interpret the variable type and assign accordingly. Instead of this, use the actual variable type definition. The Units class is also not available in SolversLib and is instead a WPILib utility.


# Trajectory Constraints

In the [previous article](/pathing/pure-pursuit/trajectory/trajectory-generation), you might have noticed that no custom constraints were added when generating the trajectories. Custom constraints allow users to impose more restrictions on the velocity and acceleration at points along the trajectory based on location and curvature.

For example, a custom constraint can keep the velocity of the trajectory under a certain threshold in a certain region or slow down the robot near turns for stability purposes.

## Provided Constraints

SolversLib includes a set of predefined constraints that users can utilize when generating trajectories. The list of SolversLib-provided constraints is as follows:

* `CentripetalAccelerationConstraint`: Limits the centripetal acceleration of the robot as it traverses along the trajectory. This can help slow down the robot around tight turns.
* `DifferentialDriveKinematicsConstraint`: Limits the velocity of the robot around turns such that no wheel of a differential-drive robot goes over a specified maximum velocity.
* `DifferentialDriveVoltageConstraint`: Limits the acceleration of a differential drive robot such that no commanded voltage goes over a specified maximum.
* `MecanumDriveKinematicsConstraint`: Limits the velocity of the robot around turns such that no wheel of a [mecanum](/features/drivebases#mecanum)-drive robot goes over a specified maximum velocity.
* `SwerveDriveKinematicsConstraint`: Limits the velocity of the robot around turns such that no wheel of a swerve-drive robot goes over a specified maximum velocity.

### Note

The `DifferentialDriveVoltageConstraint` only ensures that theoretical voltage commands do not go over the specified maximum using a [feedforward model](/features/controllers#feedforward-control). If the robot were to deviate from the reference while tracking, the commanded voltage may be higher than the specified maximum.

## Creating a Custom Constraint

Users can create their own constraint by implementing the `TrajectoryConstraint` [interface](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/trajectory/constraint/TrajectoryConstraint.java).

```java
@Override
public double getMaxVelocityMetersPerSecond(
  Pose2d poseMeters,
  double curvatureRadPerMeter,
  double velocityMetersPerSecond) {
  // code here
}

@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
  Pose2d poseMeters,
  double curvatureRadPerMeter,
  double velocityMetersPerSecond) {
  // code here
}
```

The `MaxVelocity` method should return the maximum allowed velocity for the given pose, curvature, and original velocity of the trajectory without any constraints. The `MinMaxAcceleration` method should return the minimum and maximum allowed acceleration for the given pose, curvature, and constrained velocity.


# Manipulating Trajectories

Once a trajectory has been generated, you can retrieve information from it using certain methods. These methods will be useful when writing code to follow these trajectories.

## Getting the Total Duration of the Trajectory

Because all trajectories have timestamps at each point, the amount of time it should take for a robot to traverse the entire trajectory is predetermined. The`getTotalTimeSeconds()`method can be used to determine the time it takes to traverse the trajectory.

```java
// Get the total time of the trajectory in seconds
double duration = trajectory.getTotalTimeSeconds();
```

## Sampling the Trajectory

The trajectory can be sampled at various timesteps to get the pose, velocity, and acceleration at that point. The `sample(double timeSeconds)` method can be used to sample the trajectory at any timestep. The parameter refers to the amount of time passed since 0 seconds (the starting point of the trajectory).

```java
// Sample the trajectory at 1.2 seconds. This represents where the robot
// should be after 1.2 seconds of traversal.
Trajectory.State point = trajectory.sample(1.2);
```

The sample has several pieces of information about the sample point:

* `t`: The time elapsed from the beginning of the trajectory up to the sample point.
* `velocity`: The velocity at the sample point.
* `acceleration`: The acceleration at the sample point.
* `pose`: The pose (x, y, heading) at the sample point.
* `curvature`: The curvature (rate of change of heading with respect to distance along the trajectory) at the sample point.

Note: The angular velocity at the sample point can be calculated by multiplying the velocity by the curvature.


# Transforming Trajectories

Trajectories can be transformed from one coordinate system to another and moved within a coordinate system using the `relativeTo` and the `transformBy` methods. These methods are useful for moving trajectories within space, or redefining an already existing trajectory in another frame of reference.

### Note

Neither of these methods changes the shape of the original trajectory.

## The `relativeTo` Method

The `relativeTo` method is used to redefine an already existing trajectory in another frame of reference. This method takes one argument: a pose, (via a `Pose2d` object) that is defined with respect to the current coordinate system, that represents the origin of the new coordinate system.

For example, a trajectory defined in coordinate system A can be redefined in coordinate system B, whose origin is at (2, 2, 30 degrees) in coordinate system A, using the `relativeTo` method.

```java
Pose2d bOrigin = new Pose2d(2, 2, Rotation2d.fromDegrees(30));
Trajectory bTrajectory = aTrajectory.relativeTo(bOrigin);
```

![](/files/raCpSj7Ja8QrFtxvwrMZ)

In the diagram above, the original trajectory (`aTrajectory` in the code above) has been defined in coordinate system A, represented by the black axes. The red axes, located at (2, 2) and 30° with respect to the original coordinate system, represent coordinate system B. Calling `relativeTo` on `aTrajectory` will redefine all poses in the trajectory to be relative to coordinate system B (red axes).

## The `transformBy` Method

The `transformBy` method can be used to move (i.e. translate and rotate) a trajectory within a coordinate system. This method takes one argument: a transform (via a `Transform2d` object) that maps the current initial position of the trajectory to a desired initial position of the same trajectory.

For example, one may want to transform a trajectory that begins at (2, 2, 30 degrees) to make it begin at (4, 4, 50 degrees) using the `transformBy` method.

```java
Transform2d transform = new Pose2d(4, 4, Rotation2d.fromDegrees(50)).minus(trajectory.getInitialPose());
Trajectory newTrajectory = trajectory.transformBy(transform);
```

![](/files/xfUyvSlK1FFOTOgQVARC)

In the diagram above, the original trajectory, which starts at (2, 2) and at 30° is visible in blue. After applying the transform above, the resultant trajectory’s starting location is changed to (4, 4) at 50°. The resultant trajectory is visible in orange.


# Ramsete Controller

import com.seattlesolvers.solverslib.controller.wpilibcontroller.RamseteController

The Ramsete Controller is a trajectory tracker that is built in to SolversLib. This tracker can be used to accurately track trajectories with correction for minor disturbances.

Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law in addition to the linear ones we have used so far like PID? If we use the original approach with PID controllers for left and right position and velocity states, the controllers only deal with the local pose. If the robot deviates from the path, there is no way for the controllers to correct and the robot may not reach the desired global pose. This is due to multiple endpoints existing for the robot which have the same encoder path arc lengths.

Instead of using wheel path arc lengths (which are in the robot's local coordinate frame), nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this extra information to guide a linear reference tracker like the PID controllers back in by adjusting the references of the PID controllers.

## Constructing the Ramsete Controller Object

The Ramsete controller should be initialized with two gains, namely `b` and `zeta`. Larger values of `b` make convergence more aggressive like a proportional term whereas larger values of `zeta` provide more damping in the response. These controller gains only dictate how the controller will output adjusted velocities. It does NOT affect the actual velocity tracking of the robot. This means that these controller gains are generally robot-agnostic.

## Getting Adjusted Velocities

The Ramsete controller returns “adjusted velocities” so that the when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new goal. The goal comprises of a desired pose, desired linear velocity, and desired angular velocity. Furthermore, the current position of the robot should also be updated periodically. The controller uses these four arguments to return the adjusted linear and angular velocity. Users should command their robot to these linear and angular velocities to achieve optimal trajectory tracking.

The “goal pose” represents the position that the robot should be at at a particular timestep when tracking the trajectory. It does NOT represent the final endpoint of the trajectory.

The controller can be updated using the `calculate` method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter. For the other parameters, one of these overloads takes in the goal as three separate parameters (pose, linear velocity, and angular velocity) whereas the other overload accepts a `Trajectory.State` object, which contains information about the goal pose. For its ease, users should use the latter method when tracking trajectories.

```java
Trajectory.State goal = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
```

These calculations should be performed at every loop iteration, with an updated robot position and goal.

## Using the Adjusted Velocities

The adjusted velocities are of type `ChassisSpeeds`, which contains a `vx` (linear velocity in the forward direction), a `vy` (linear velocity in the sideways direction), and an `omega` (angular velocity around the center of the robot frame). Because the Ramsete controller is a controller for non-holonomic robots (robots which cannot move sideways), the adjusted speeds object has a `vy` of zero.

The returned adjusted speeds can be converted to usable speeds using the kinematics classes for your [drivetrain](/features/drivebases) type. For example, the adjusted velocities can be converted to left and right velocities for a differential drive using a `DifferentialDriveKinematics` object.

```java
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds);
double left = wheelSpeeds.leftMetersPerSecond;
double right = wheelSpeeds.rightMetersPerSecond;
```

Because these new left and right velocities are still speeds and not voltages, two PID controllers, one for each side, may be used to track these velocities.


# Command System

package com.seattlesolvers.solverslib.command

The current command system for SolversLib is modeled closely after that of [WPILib](https://docs.wpilib.org/en/latest/docs/software/commandbased/index.html). Some of the following information may be copied from the source material.

The command-based paradigm is one that allows programming to follow a set design pattern. The specific command system that SolversLib uses follows a declarative programming style. The emphasis is, instead, on what the program *should* do rather than how to do it. This minimizes the iteration-by-iteration robot logic needed to write out a certain action. Very simply, you can bind some actions to buttons/triggers such as the example below:

```java
aButton.whenPressed(intake::run);
```

Or, instead of binding to a button, you can simply schedule an action to occur by registering it.

## Terminology

Let's discuss some of the basic terminology for the paradigm.

### Subsystem

A **subsystem** is the basic unit of robot organization in the design-based paradigm. Subsystems [encapsulate](https://en.wikipedia.org/wiki/Encapsulation_\(computer_programming\)) lower-level robot hardware (such as motor controllers, sensors, and/or pneumatic actuators), and define the interfaces through which that hardware can be accessed by the rest of the robot code. Subsystems allow users to “hide” the internal complexity of their actual hardware from the rest of their code - this both simplifies the rest of the robot code, and allows changes to the internal details of a subsystem without also changing the rest of the robot code. Subsystems implement the `Subsystem` interface.

### Command

A **command** defines high-level robot actions or behaviors that utilize the methods defined by the subsystems. A command is a simple [state machine](https://en.wikipedia.org/wiki/Finite-state_machine) that is either initializing, executing, ending, or idle. Users write code specifying which action should be taken in each state. Simple commands can be composed into “command groups” to accomplish more-complicated tasks. Commands, including command groups, implement the `Command` interface.

#### How Commands Are Run

For a more detailed explanation, please see the [Command Scheduler](/command-base/command-system/command-scheduler).

Commands are run by the `CommandScheduler`, which is a singleton at the base of the command system. The command scheduler is in charge of polling buttons for new commands to schedule, checking the resources required by those commands to avoid conflicts, executing currently-scheduled commands, and removing commands that have finished or been interrupted. The scheduler’s `run()` method may be called from any place in the user’s code.

Multiple commands can run concurrently, as long as they do not require the same resources on the robot. Resource management is handled on a per-subsystem basis: commands may specify which subsystems they interact with, and the scheduler will never schedule more than one command requiring a given subsystem at a time. This ensures that, for example, users will not end up with two different pieces of code attempting to set the same motor controller to different output values. If a new command is scheduled that requires a subsystem that is already in use, it will either interrupt the currently-running command that requires that subsystem (if the command has been scheduled as interruptible), or else it will not be scheduled.

Subsystems also can be associated with “default commands” that will be automatically scheduled when no other command is currently using the subsystem. This is useful for continuous “background” actions such as controlling the robot drive, or keeping an arm held at a setpoint.

When a command is scheduled, its `initialize()` method is called once. Its `execute()` method is then called once per call to `CommandScheduler.getInstance().run()`. A command is unscheduled and has its `end(boolean interrupted)` method called when either its `isFinished()` method returns true, or else it is interrupted (either by another command with which it shares a required subsystem, or by being canceled).

#### Command Groups

It is often desirable to build complex commands from simple pieces. This is achievable by [composing](https://en.wikipedia.org/wiki/Object_composition) commands into “command groups.” A [command group](/command-base/command-system/command-groups) is a command that contains multiple commands within it, which run either in parallel or in sequence. The command-based library provides several types of command groups for teams to use, and users are encouraged to write their own, if desired. As command groups themselves implement the `Command` interface, they are [recursively composable](https://en.wikipedia.org/wiki/Object_composition#Recursive_composition) - one can include command groups *within* other command groups. This provides an extremely powerful way of building complex robot actions with a simple library.


# Subsystems

package com.seattlesolvers.solverslib.command.Subsystem

Subsystems are the basic unit of robot organization in the command-based paradigm. A subsystem is an abstraction for a collection of robot hardware that *operates together as a unit*. Subsystems [encapsulate](https://en.wikipedia.org/wiki/Encapsulation_\(computer_programming\)) this hardware, “hiding” it from the rest of the robot code (e.g. commands) and restricting access to it except through the subsystem’s public methods. Restricting the access in this way provides a single convenient place for code that might otherwise be duplicated in multiple places (such as scaling motor outputs or checking limit switches) if the subsystem internals were exposed. It also allows changes to the specific details of how the subsystem works (the “implementation”) to be isolated from the rest of robot code, making it far easier to make substantial changes if/when the design constraints change.

Subsystems also serve as the backbone of the `CommandScheduler`’s resource management system. Commands may declare resource requirements by specifying which subsystems they interact with; the scheduler will never concurrently schedule more than one command that requires a given subsystem. An attempt to schedule a command that requires a subsystem that is already-in-use will either interrupt the currently-running command (if the command has been scheduled as interruptible), or else be ignored.

Subsystems can be associated with “default commands” that will be automatically scheduled when no other command is currently using the subsystem. This is useful for continuous “background” actions such as controlling the robot drive, or keeping an arm held at a setpoint. Similar functionality can be achieved in the subsystem’s `periodic()` method, which is run once per run of the scheduler; teams should try to be consistent within their codebase about which functionality is achieved through either of these methods. Subsystems are represented in the command-based library by the Subsystem interface.

## Creating a Subsystem

The recommended method to create a subsystem for most users is to subclass the abstract `SubsystemBase` class, as seen in the command-based template:

```java
import com.seattlesolvers.solverslib.command.SubsystemBase;

public class ExampleSubsystem extends SubsystemBase {
  /**
   * Creates a new ExampleSubsystem.
   */
  public ExampleSubsystem() {

  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}
```

This class contains automatically calls the `register()` method in its constructor to register the subsystem with the scheduler (this is necessary for the `periodic()` method to be called when the scheduler runs).

Advanced users seeking more flexibility may simply create a class that implements the `Subsystem` interface.

## Creating a Simple Subsystem

Subsystems are easy to create. They combine different sets of hardware to produce either multiple or one particular task. Hence why they are the basic unit of organization in the command system. Below is a simple example:

```java
package org.firstinspires.ftc.robotcontroller.external.samples.CommandSample;

import com.seattlesolvers.solverslib..command.SubsystemBase;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

/**
 * A gripper mechanism that grabs a stone from the quarry.
 * Centered around the Skystone game for FTC that was done in the 2019
 * to 2020 season.
 */
public class GripperSubsystem extends SubsystemBase {

    private final Servo mechRotation;

    public GripperSubsystem(final HardwareMap hMap, final String name) {
        mechRotation = hMap.get(Servo.class, name);
    }

    /**
     * Grabs a stone.
     */
    public void grab() {
        mechRotation.setPosition(0.76);
    }

    /**
     * Releases a stone.
     */
    public void release() {
        mechRotation.setPosition(0);
    }

}
```

## Setting Default Commands

“Default commands” are commands that run automatically whenever a subsystem is not being used by another command.

Setting a default command for a subsystem is very easy; one simply calls `CommandScheduler.getInstance().setDefaultCommand()`, or, more simply, the `setDefaultCommand()` method of the `Subsystem` interface which can be called in the OpMode:

```java
CommandScheduler.getInstance().setDefaultCommand(exampleSubsystem, exampleCommand);
```

```java
exampleSubsystem.setDefaultCommand(exampleCommand);
```


# Command

package com.seattlesolvers.solverslib.command.Command

Commands are simple state machines that perform high-level robot functions using the methods defined by subsystems. Commands can be either idle, in which they do nothing, or scheduled, in which the scheduler will execute a specific set of the command’s code depending on the state of the command. The `CommandScheduler` recognizes scheduled commands as being in one of three states: initializing, executing, or ending. Commands specify what is done in each of these states through the `initialize()`, `execute()` and `end()` methods. Commands are represented in the command-based library by the `Command` interface.

## Creating Commands

Similarly to subsystems, the recommended method for most users to create a command is to subclass the abstract `CommandBase` class, as seen in the command-based template.

```java
import com.seattlesolvers.solverslib.command.CommandBase;

/**
 * An example command that uses an example subsystem.
 */
public class ExampleCommand extends CommandBase {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final ExampleSubsystem m_subsystem;

  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public ExampleCommand(ExampleSubsystem subsystem) {
    m_subsystem = subsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(subsystem);
  }
}
```

As before, this contains several convenience features. It automatically overrides the `getRequirements()` method for users, returning a list of requirements that is empty by default, but can be added to with the `addRequirements()` method.

Also as before, advanced users seeking more flexibility are free to simply create their own class implementing the `Command` interface.

To schedule a command to the scheduler, you will need to call the `schedule()` method of the command instance.

<pre class="language-java"><code class="lang-java"><strong>m_command.schedule();
</strong></code></pre>

To schedule a command as uninterruptible, you can simply schedule it as `false`. The default schedule is `true` (interruptible).

```java
m_command.schedule(false);
```

## The Structure of a Command

While subsystems are fairly freeform, and may generally look like whatever the user wishes them to, commands are quite a bit more constrained. Command code must specify what the command will do in each of its possible states. This is done by overriding the `initialize()`, `execute()`, and `end()` methods. Additionally, a command must be able to tell the scheduler when (if ever) it has finished execution - this is done by overriding the `isFinished()` method. All of these methods are defaulted to reduce clutter in user code: `initialize()`, `execute()`, and `end()` are defaulted to simply do nothing, while `isFinished()` is defaulted to return false (resulting in a command that never ends).

### Initialization

The `initialize()` method is run exactly once per time a command is scheduled, as part of the scheduler’s `schedule()` method. The scheduler’s `run()` method does not need to be called for the `initialize()` method to run. The initialize block should be used to place the command in a known starting state for execution. It is also useful for performing tasks that only need to be performed once per time scheduled, such as setting motors to run at a constant speed.

### Execution

The `execute()` method is called repeatedly while the command is scheduled, whenever the scheduler’s `run()` method is called. The execute block should be used for any task that needs to be done continually while the command is scheduled, such as updating motor outputs to match joystick inputs, or using the output of a control loop.

### Ending

The `end()` method is called once when the command ends, whether it finishes normally (i.e. `isFinished()` returned true) or it was interrupted (either by another command or by being explicitly canceled). The method argument specifies the manner in which the command ended; users can use this to differentiate the behavior of their command end accordingly. The end block should be used to “wrap up” command state in a neat way, such as setting motors back to zero or reverting a solenoid actuator to a “default” state.

### Specifying End Conditions

The `isFinished()` method is called repeatedly while the command is scheduled, whenever the scheduler’s `run()` method is called. As soon as it returns true, the command’s `end()` method is called and it is unscheduled. The `isFinished()` method is called *after* the `execute()` method, so the command *will* execute once on the same iteration that it is unscheduled.

### Simple Command Example

Taking the gripper example from the [Subsystem](/command-base/command-system/subsystems) page, we can develop the following action to grab a stone from the quarry:

```java
import com.seattlesolvers.solverslib.command.CommandBase;

/**
 * A simple command that grabs a stone with the
 * {@link GripperSubsystem}.  Written explicitly for
 * pedagogical purposes. Actual code should inline a
 * command this simple with {@link
 * com.seattlesolvers.solverslib.command.InstantCommand}.
 */
public class GrabStone extends CommandBase {

    // The subsystem the command runs on
    private final GripperSubsystem m_gripperSubsystem;

    public GrabStone(GripperSubsystem subsystem) {
        m_gripperSubsystem = subsystem;
        addRequirements(m_gripperSubsystem);
    }

    @Override
    public void initialize() {
        m_gripperSubsystem.grab();
    }

    @Override
    public boolean isFinished() {
        return true;
    }

}
```

Notice that, here, the gripper subsystem is passed into the constructor in order to produce the command action. This is called [dependency injection](https://en.wikipedia.org/wiki/Dependency_injection), and allows users to avoid declaring their subsystems as global variables. This is widely accepted as a best-practice.

Notice also that the above command calls the subsystem method once from initialize, and then immediately ends (as `isFinished()` simply returns true).

Below is a more complex example of a custom command.

```java
import com.seattlesolvers.solverslib.command.CommandBase;

import java.util.function.DoubleSupplier;

/**
 * A command to drive the robot with joystick input
 * (passed in as {@link DoubleSupplier}s). Written
 * explicitly for pedagogical purposes.
 */
public class DefaultDrive extends CommandBase {

    private final DriveSubsystem m_drive;
    private final DoubleSupplier m_forward;
    private final DoubleSupplier m_rotation;

    /**
     * Creates a new DefaultDrive.
     *
     * @param subsystem The drive subsystem this command will run on.
     * @param forward The control input for driving forwards/backwards
     * @param rotation The control input for turning
     */
    public DefaultDrive(DriveSubsystem subsystem,
        DoubleSupplier forward, DoubleSupplier rotation) {
        m_drive = subsystem;
        m_forward = forward;
        m_rotation = rotation;
        addRequirements(m_drive);
    }

    @Override
    public void execute() {
        m_drive.drive(
            m_forward.getAsDouble(),
            m_rotation.getAsDouble()
        );
    }

}
```

Notice that this command does not override `isFinished()`, and thus will never end.


# Command Groups

package com.seattlesolvers.solverslib.command.CommandGroup

Individual commands are capable of accomplishing a large variety of robot tasks, but the simple three-state format can quickly become cumbersome when more advanced functionality requiring extended sequences of robot tasks or coordination of multiple robot subsystems is required. In order to accomplish this, users are encouraged to use the powerful command group functionality included in the command-based library.

As the name suggests, command groups are combinations of multiple commands. The act of combining multiple objects (such as commands) into a bigger object is known as [composition](https://en.wikipedia.org/wiki/Object_composition). Command groups *compose* multiple commands into a *composite* command. This allows code to be kept much cleaner and simpler, as the individual *component* commands may be written independently of the code that combines them, greatly reducing the amount of complexity at any given step of the process.

Most importantly, however, command groups *are themselves commands* - they implement the `Command` interface. This allows command groups to be [recursively composed](https://en.wikipedia.org/wiki/Object_composition#Recursive_composition) - that is, a command group may contain *other command groups* as components.

## Types of Command Groups

The command-based library supports four basic types of command groups: `SequentialCommandGroup`, `ParallelCommandGroup`, `ParallelRaceGroup`, and `ParallelDeadlineGroup`. Each of these command groups combines multiple commands into a composite command - however, they do so in different ways:

### SequentialCommandGroup

A `SequentialCommandGroup` runs a list of commands in sequence - the first command will be executed, then the second, then the third, and so on until the list finishes. The sequential group finishes after the last command in the sequence finishes. It is therefore usually important to ensure that each command in the sequence does actually finish (if a given command does not finish, the next command will never start!).

### ParallelCommandGroup

A `ParallelCommandGroup` runs a set of commands concurrently - all commands will execute at the same time. The parallel group will end when all commands have finished.

### ParallelRaceGroup

A `ParallelRaceGroup` is much like a `ParallelCommandgroup`, in that it runs a set of commands concurrently. However, the race group ends as soon as any command in the group ends - all other commands are interrupted at that point.

### ParallelDeadlineGroup

A `ParallelDeadlineGroup`also runs a set of commands concurrently. However, the deadline group ends when a *specific* command (the “deadline”) ends, interrupting all other commands in the group that are still running at that point.

## Creating Command Groups

Users have several options for creating command groups. One way - similar to the previous implementation of the command-based library - is to subclass one of the command group classes. Below is an example of a command group:

```java
import com.seattlesolvers.solverslib.command.SequentialCommandGroup;

/**
 * A complex auto command that drives forward,
 * releases a stone, and then drives backward.
 */
public class ReleaseAndBack extends SequentialCommandGroup {

    private static final double INCHES = 3.0;
    private static final double SPEED = 0.5;

    /**
     * Creates a new ReleaseAndBack command group.
     *
     * @param drive The drive subsystem this command will run on
     * @param grip The gripper subsystem this command will run on
     */
    public ReleaseAndBack(DriveSubsystem drive, GripperSubsystem grip)
    {
        addCommands(
                new DriveDistance(INCHES, SPEED, drive),
                new ReleaseStone(grip),
                new DriveDistance(INCHES, SPEED, drive)
        );
        addRequirements(drive, grip);
    }

}
```

The `addCommands()` method adds commands to the group, and is present in all four types of command group.

## Inline Command Groups

Command groups can be used without subclassing at all: one can simply pass in the desired commands through the constructor:

```java
new SequentialCommandGroup(new FooCommand(), new BarCommand());
```

This is called an inline command definition, and is very handy for circumstances where command groups are not likely to be reused, and writing an entire class for them would be wasteful.

## Recursive Composition of Command Groups

As mentioned earlier, command groups are [recursively composable](https://en.wikipedia.org/wiki/Object_composition#Recursive_composition) - since command groups are themselves commands, they may be included as components of other command groups. This is an extremely powerful feature of command groups, and allows users to build very complex robot actions from simple pieces. For example, consider the following code:

```java
new SequentialCommandGroup(
   new DriveToGoal(m_drive),
   new ParallelCommandGroup(
      new RaiseElevator(m_elevator),
      new SetWristPosition(m_wrist)),
   new ScoreTube(m_wrist));
```

Notice how the recursive composition allows the embedding of a parallel control structure within a sequential one. Notice also that this entire, more-complex structure, could be again embedded in another structure. Composition is an extremely powerful tool, and one that users should be sure to use extensively.

## Command Group and Requirements

As command groups are commands, they also must declare their requirements. However, users are not required to specify requirements manually for command groups - requirements are automatically inferred from the commands included. As a rule, *command groups include the union of all of the subsystems required by their component commands.* Thus, the `ReleaseAndBack` shown previously will require both the drive subsystem and the gripper subsystem of the robot.

Additionally, requirements are enforced within all three types of parallel groups - a parallel group may *not* contain multiple commands that require the same subsystem.

Some advanced users may find this overly-restrictive - for said users, the library offers a `ScheduleCommand` class that can be used to independently “branch off” from command groups to provide finer granularity in requirement management.

## Restrictions on Command Group Components

Since command group components are run through their encapsulating command groups, errors could occur if those same command instances were independently scheduled at the same time as the group - the command would be being run from multiple places at once, and thus could end up with inconsistent internal state, causing unexpected and hard-to-diagnose behavior.

For this reason, command instances that have been added to a command group cannot be independently scheduled or added to a second command group. Attempting to do so will throw an exception and crash the user program.

Advanced users who wish to re-use a command instance and are *certain* that it is safe to do so may bypass this restriction with the `clearGroupedCommand()` method in the `CommandGroupBase` class.


# Command Scheduler

package com.seattlesolvers.solverslib.command.CommandScheduler

The `CommandScheduler` is the class responsible for actually running commands. Each iteration, the scheduler polls all registered buttons, schedules commands for execution accordingly, runs the command bodies of all scheduled commands, and ends those commands that have finished or are interrupted.

The `CommandScheduler` also runs the `periodic()` method of each registered `Subsystem`.

## Using the Command Scheduler

The `CommandScheduler` is a *singleton*, meaning that it is a globally-accessible class with only one instance. Accordingly, in order to access the scheduler, users must call the `CommandScheduler.getInstance()` command.

For the most part, users do not have to call scheduler methods directly - almost all important scheduler methods have convenience wrappers elsewhere (e.g. in the `Command` and `Subsystem` interfaces).

However, there is one exception: users *must* call `CommandScheduler.getInstance().run()` from the periodic method of their opmode. If this is not done, the scheduler will never run, and the command framework will not work.

To schedule a command, users call the `schedule()` method. This method takes a command (and, optionally, a specification as to whether that command is interruptible), and attempts to add it to list of currently-running commands, pending whether it is already running or whether its requirements are available. If it is added, its `initialize()` method is called.

## The Scheduler Run Sequence

The `initialize()` method of each `Command` is called when the command is scheduled, which is not necessarily when the scheduler runs (unless that command is bound to a button).

What does a single iteration of the scheduler’s `run()` method actually do? The following section walks through the logic of a scheduler iteration.

### Step 1: Run Subsystem Periodic Methods

First, the scheduler runs the `periodic()` method of each registered `Subsystem`.

### Step 2: Poll Command Scheduling Triggers

Secondly, the scheduler polls the state of all registered triggers to see if any new commands that have been bound to those triggers should be scheduled. If the conditions for scheduling a bound command are met, the command is scheduled and its `initialize()` method is run.

### Step 3: Run/Finish Scheduled Commands

Thirdly, the scheduler calls the `execute()` method of each currently-scheduled command, and then checks whether the command has finished by calling the `isFinished()` method. If the command has finished, the `end()` method is also called, and the command is de-scheduled and its required subsystems are freed.

Note that this sequence of calls is done in order for each command - thus, one command may have its `end()` method called before another has its `execute()` method called. Commands are handled in the order they were scheduled.

### Step 4: Schedule Default Commands

Finally, any registered `Subsystem` has its default command scheduled (if it has one). Note that the `initialize()` method of the default command will be called at this time.

## Disabling the Scheduler

The scheduler can be disabled by calling `CommandScheduler.getInstance().disable()`. When disabled, the scheduler’s `schedule()` and `run()` commands will not do anything.

The scheduler may be re-enabled by calling `CommandScheduler.getInstance().enable()`.

If you want to reset the scheduler (clear the instance), call `CommandScheduler.getInstance().reset()`.

## Command Event Methods

Occasionally, it is desirable to have the scheduler execute a custom action whenever a certain command event (initialization, execution, or ending) occurs. This can be done with the following three methods:

### onCommandInitialize

The `onCommandInitialize` method runs a specified action whenever a command is initialized.

### onCommandExecute

The `onCommandExecute` method runs a specified action whenever a command is executed.

### onCommandFinish

The `onCommandFinish` method runs a specified action whenever a command finishes normally (i.e. the `isFinished()` method returned true).

### onCommandInterrupt

The `onCommandInterrupt` method runs a specified action whenever a command is interrupted (i.e. by being explicitly canceled or by another command that shares one of its requirements).


# Binding Commands to Triggers

package com.seattlesolvers.solverslib.command.button

Apart from autonomous commands, which are scheduled at the start of the autonomous period, and default commands, which are automatically scheduled whenever their subsystem is not currently in-use, the most common way to run a command is by binding it to a triggering event, such as a button being pressed by a human operator. The command-based paradigm makes this extremely easy to do.

As mentioned earlier, command-based is a [declarative](https://en.wikipedia.org/wiki/Declarative_programming) paradigm. Accordingly, binding buttons to commands is done declaratively; the association of a button and a command is “declared” once, during robot initialization. The library then does all the hard work of checking the button state and scheduling (or cancelling) the command as needed, behind-the-scenes. Users only need to worry about designing their desired UI setup - not about implementing it!

Command binding is done through the `Trigger` class and its various `Button` subclasses.

## Trigger/Button Bindings

There are a number of bindings available for the `Trigger` class. All of these bindings will automatically schedule a command when a certain trigger activation event occurs - however, each binding has different specific behavior. `Button` and its subclasses have bindings with identical behaviors, but slightly different names that better-match a button rather than an arbitrary triggering event.

### whenActive/whenPressed

This binding schedules a command when a trigger changes from inactive to active (or, accordingly, when a button changes is initially pressed). The command will be scheduled on the iteration when the state changes, and will not be scheduled again unless the trigger becomes inactive and then active again (or the button is released and then re-pressed).

### whileActiveContinuous/whileHeld

This binding schedules a command repeatedly while a trigger is active (or, accordingly, while a button is held), and cancels it when the trigger becomes inactive (or when the button is released). Note that scheduling an already-running command has no effect; but if the command finishes while the trigger is still active, it will be re-scheduled.

### whileActiveOnce/whenHeld

This binding schedules a command when a trigger changes from inactive to active (or, accordingly, when a button is initially pressed) and cancels it when the trigger becomes inactive again (or the button is released). The command will *not* be re-scheduled if it finishes while the trigger is still active.

### whenInactive/whenReleased

This binding schedules a command when a trigger changes from active to inactive (or, accordingly, when a button is initially released). The command will be scheduled on the iteration when the state changes, and will not be re-scheduled unless the trigger becomes active and then inactive again (or the button is pressed and then re-released).

### toggleWhenActive/toggleWhenPressed

This binding toggles a command, scheduling it when a trigger changes from inactive to active (or a button is initially pressed), and cancelling it under the same condition if the command is currently running. Note that while this functionality is supported, toggles are *not* a highly-recommended option for user control, as they require the driver to mentally keep track of the robot state.

Since v1.2.0, courtesy of Ethan Leitner, the toggle binding has an additional option of switching between two commands. This is a replacement of the `ConditionalCommand` option for toggling. Simply pass two commands into the method instead of one, and the binding will toggle between those two commands.

### cancelWhenActive/cancelWhenPressed

This binding cancels a command when a trigger changes from inactive to active (or, accordingly, when a button is initially pressed). the command is canceled on the iteration when the state changes, and will not be canceled again unless the trigger becomes inactive and then active again (or the button is released and re-pressed). Note that cancelling a command that is not currently running has no effect.

## Binding a Command to a Gamepad Button

The most-common way to trigger a command is to bind a command to a button on the gamepad.

### Creating a GamepadButton

In order to create a `GamepadButton`, we first need a `GamepadEx`.

```java
GamepadEx driverOp = new GamepadEx(gamepad1);
GamepadEx toolOp = new GamepadEx(gamepad2);
```

When the object is instantiated, users can then pass it into the `GamepadButton` class.

```java
Button exampleButton = new GamepadButton(
    driverOp, GamepadKeys.Button.A
);

// alternatively, you can use the mapped GamepadButtons
// built in to the GamepadEx class
driverOp.getGamepadButton(GamepadKeys.Button.A);
```

### Binding a Command to a Button

Putting it all together, it is very simple to bind a command to a Button.

```java
exampleButton.whenPressed(new ExampleCommand());

/* It is super useful to also use instant commands */
exampleButton.whenPressed(new InstantCommand(() -> {
    // your implementation of Runnable.run() here
}));
```

It is useful to note that the command binding methods all return the trigger/button that they were initially called on, and thus can be chained to bind multiple commands to different states of the same button. For example:

```java
exampleButton
    // Binds a FooCommand to be scheduled when the `X` button
    // of the driver gamepad is pressed
    .whenPressed(new FooCommand())
    // Binds a BarCommand to be scheduled when that same button
    // is released
    .whenReleased(new BarCommand());
    
// instantiation + binding
    
/* You can also do this when you create a button object */
Button exampleButton = new GamepadButton(
    driverOp, GamepadKeys.Button.A
    // then bind commands
).whenPressed(new FooCommand())
    .whenReleased(new BarCommand());
    
/* You can also do this with the GamepadEx button mapping */
driverOp.getGamepadButton(GamepadKeys.Button.A)
    .whenPressed(new FooCommand())
    .whenReleased(new BarCommand());
```

Remember that button binding is *declarative.* Bindings only need to be declared once, ideally some time during robot initialization. The library handles everything else.

## Composing Triggers

The `Trigger` class (including its `Button` subclasses) can be composed to create composite triggers through the `and()`, `or()`, and `negate()` methods. For example:

```java
// Binds an ExampleCommand to be scheduled when both the 'X' and
// 'Y' buttons of the driver gamepad are pressed
new GamepadButton(driverOp, GamepadKeys.Button.X)
    .and(new GamepadButton(exampleController, GamepadKeys.Button.Y))
    .whenActive(new ExampleCommand());
```

Note that these methods return a `Trigger`, not a `Button`, so the `Trigger` binding method names must be used even when buttons are composed.

## Creating Your Own Custom Trigger

While binding to buttons is by far the most common use case, advanced users may occasionally want to bind commands to arbitrary triggering events. This can be easily done by simply writing your own subclass of `Trigger` or `Button`:

```java
public class ExampleTrigger extends Trigger {
  @Override
  public boolean get() {
    // This returns whether the trigger is active
  }
}
```

Alternatively, this can also be done inline by passing a lambda to the constructor of `Trigger` or `Button`:

```java
// Here it is assumed that "condition" is an object with a
// method "get" that returns whether the trigger should be active
Trigger exampleTrigger = new Trigger(condition::get);
```

This can be used for implementing a type of "triggering condition," which one might see with a sensor. On the activation of the trigger, you can cause certain commands to be scheduled or cancelled.


# Robot and CommandOpMode

Organizing your code with the Robot class and CommandOpMode.

**NOTE**: We recommend using the [CommandOpMode](#commandopmode) for your implementation of the command-based paradigm as it is much friendlier for FTC programs.

## The Robot Class

The [Robot](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/Robot.java) class makes using the CommandScheduler so much simpler in the user's code. Similar to WPILib, the Robot class allows for making a Robot paradigm. This allows for the creation of multiple opmodes tied to one object. This is also the basis for disabling the robot, meaning we want to interrupt the commands and not allow for scheduling new ones. This is done with the static `disable()` and `enable()` methods:

```java
// disable the Robot
Robot.disable();

// enable the Robot
Robot.enable();
```

### Creating a Robot Class

The first step in using the Robot paradigm is to create your own Robot subclass. This will maintain all of the commands and subsystems you want to utilize. Also note that you can schedule and register new commands and subsystems from outside the Robot class using the `schedule()` and `register()` methods.

A major downside of the Robot class is that it shares the common commands and subsystems for every use in the OpModes. A recommendation is to create an [enum](https://docs.oracle.com/javase/tutorial/java/javaOO/enum.html) inside of your robot implementation that specifies TeleOp versus Autonomous so that when the object is constructed, it uses the desired CommandScheduler instance.

Below is an example of utilizing this feature.

```java
// ... in MyRobot.java (extends Robot)

// enum to specify opmode type
public enum OpModeType {
    TELEOP, AUTO
}

// the constructor with a specified opmode type
public MyRobot(OpModeType type) {
    if (type == OpModeType.TELEOP) {
        initTele();
    } else {
        initAuto();
    }
}

/*
 * Initialize teleop or autonomous, depending on which is used
 */
public void initTele() {
    // initialize teleop-specific scheduler
}

public void initAuto() {
    // initialize auto-specific scheduler
}


// ... in your opmode

// our robot object
Robot m_robot = new MyRobot(MyRobot.OpModeType.TELEOP);
```

A downside that cannot be fixed by this is when you want to run several different opmodes of the different types with respect to the same subsystems. This is where the [CommandOpMode](#commandopmode) comes in handy and makes the paradigm a lot more FTC-friendly.

### Running the Robot in Your OpMode

Running the robot in the opmode is simple and only requires a few lines of code and making a call to the `run()` method.

```java
// assumes we have created an enum called OpModeType as seen
// in the previous section
Robot m_robot = new MyRobot(MyRobot.OpModeType.AUTO);

// run the command scheduler tied to that robot instance
while (opModeIsActive() && !isStopRequested()) {
    m_robot.run();
}
m_robot.reset();    // resets the scheduler instance
```

## CommandOpMode

The [CommandOpMode](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/CommandOpMode.java) is the center of the FTC-centric command-based paradigm. It makes everything simpler for the user and greatly decreases the amount of code needed to run everything. Unlike the Robot class, it is opmode-specific, so it does not store a common reference to subsystems. If desired, the user can create a map that houses all of the referenced hardware, but this is currently not a feature offered (but totally supported) by SolversLib.

If desired, the user can override the methods from the CommandOpMode. Similarly to the Robot class, you can disable the CommandOpMode with static methods (which actually disable the Robot class, which is what is referenced in the CommandScheduler for `runsWhenDisabled()` commands.

### Initializing your Hardware

CommandOpMode is an abstract class, which means the user must create their own implementation of it. The only method that needs to be implemented is the `initialize()` method, which instantiates all of the hardware and commands to be run by the scheduler. The CommandOpMode class already implements the `runOpMode()` method and runs the scheduler. After the opmode is no longer active or a stop is requested, it resets the instance of the scheduler so a new opmode can be run with a fresh instance. This is very nonintuitive, but it works as designed (setting the singleton instance to null, where a new instance is then created upon calling `getInstance()`.

```java
// in your implementation of CommandOpMode

@Override
public void initialize() {
    // iniialize hardware

    // schedule all commands
    schedule(commands...);
    // register unregistered subsystems
    register(subsystems...);
}
```

That is functionally all that needs to be done, everything else is done for the user internally. You can find a sample project utilizing CommandOpMode [here](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PurePursuitSample.java).

### preRun()

As of SolversLib 0.3.5, `CommandOpMode` also has an optional `preRun()` method that you can override. It is called exactly once after the "Play" button is pressed on the Driver Station — after the init phase ends, but before the main `run()` loop starts. This makes it a good place for one-time setup that should only happen when the OpMode actually starts, such as scheduling your autonomous routine or resetting timers.

Note that if the OpMode is stopped while it is still in the init phase, `preRun()` (and the `run()` loop) will not be called.

```java
// in your implementation of CommandOpMode

@Override
public void preRun() {
    // runs once after "Play" is pressed, before the run() loop starts
}
```


# Convenience Features

Using SolversLib-provided Commands to Enhance Your Program

SolversLib offers convenience features to make your paradigm program more compact. The idea is that it improves the overall structure of your program. Some of these convenience commands are just that: for convenience. This does not mean they are exceptional. Plenty are simplified for minimal competitive use, such as the [`PurePursuitCommand`](/pathing/pure-pursuit#using-the-pure-pursuit-command).

## Framework Commands

Framework commands exist to decrease the amount of program needed for simplistic tasks, like updating a number. Rather than having the user create an entire command for one simplistic task (which when done for multiple menial tasks builds up and makes the structure fairly disheveled), the user can utilize framework commands.

### InstantCommand

`InstantCommand` is a versatile framework command that on initialization runs some task and then ends on that same iteration of the CommandScheduler's `run()` method. This is especially useful for button-triggered events.

```java
GamepadEx toolOp = new GamepadEx(gamepad2);

toolOp.getGamepadButton(GamepadKeys.Button.A)
    .whenPressed(new InstantCommand(() -> {
        // your implementation of run() here
    }));

/* AN EXAMPLE */

Motor intakeMotor = new Motor(hardwareMap, "intake");

toolOp.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
    .whileHeld(new InstantCommand(() -> {
        intakeMotor.set(0.75);
    }))
    .whenReleased(new InstantCommand(intakeMotor::stopMotor));
```

You should actually use subsystems here instead of the motor object. That way we can add the subsystem's requirements to the `InstantCommand`. Below is a proper example.

{% code title="Intake.java" %}

```java
/**
 * This is a pedagogical intake subsystem for
 * a universal intake consisting of one motor
 * that drives a belt that connects to a pulley
 * that drives a PVC tube.
 */
public class Intake extends SubsystemBase {
    private Motor m_intakeMotor;
    
    public Intake(Motor intakeMotor) {
        m_intakeMotor = intakeMotor;
    }
    
    public void run() {
        m_intakeMotor.set(0.75);
    }
    
    public void stop() {
        m_intakeMotor.stopMotor();
    }
}
```

{% endcode %}

And then we can set up our command bindings as such:

```java
/* in your opmode */

Motor intakeMotor = new Motor(hardwareMap, "intake");
Intake intake = new Intake(intakeMotor);

toolOp.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
    // the second parameter is a varargs of subsystems
    // to require
    .whileHeld(new InstantCommand(intake::run, intake))
    .whenReleased(new InstantCommand(intake::stop, intake));
```

This removes a lot of unnecessary clutter of commands since in a custom implementation the user would have to define a command for both running the intake and stopping it. With `InstantCommand`, the amount of code on the user-side is dramatically reduced.

### RunCommand

As opposed to an `InstantCommand`, a `RunCommand` runs a given method in its execute phase. This is useful for `PerpetualCommand`s, default commands, and simple commands like driving a robot.

```java
/* in your opmode */

Motor intakeMotor = new Motor(hardwareMap, "intake");
Intake intake = new Intake(intakeMotor);

toolOp.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
    // the second parameter is a varargs of subsystems
    // to require
    .whileHeld(new RunCommand(intake::run, intake))
    .whenReleased(new InstantCommand(intake::stop, intake));
    
// say we have a drive subsystem
// with a drive method
schedule(new RunCommand(driveSubsystem::drive, driveSubsystem));
```

### ConditionalCommand

`ConditionalCommand` has a wide variety of uses. `ConditionalCommand` takes two commands and runs one when supplied a value of true, and another when supplied a value of false.

One use is for making a toggle between commands without using the inactive or active states of the `toggleWhenPressed()` binding. Let's update our intake to have two more methods we can use for this toggling feature:

{% code title="Intake.java" %}

```java
/**
 * This is a pedagogical intake subsystem for
 * a universal intake consisting of one motor
 * that drives a belt that connects to a pulley
 * that drives a PVC tube.
 */
public class Intake extends SubsystemBase {
    private Motor m_intakeMotor;
    private boolean m_active;
    
    public Intake(Motor intakeMotor) {
        m_intakeMotor = intakeMotor;
        m_active = true;
    }
    
    // switch the toggle
    public void toggle() {
        m_active = !m_active;
    }
    
    // return the active state
    public boolean active() {
        return m_active;
    }
    
    public void run() {
        m_intakeMotor.set(0.75);
    }
    
    public void stop() {
        m_intakeMotor.stopMotor();
    }
}
```

{% endcode %}

We can then use a conditional command in a trigger binding to produce a toggling effect once pressed:

```java
/* in your opmode */

Motor intakeMotor = new Motor(hardwareMap, "intake");
Intake intake = new Intake(intakeMotor);

toolOp.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
    .whenPressed(new ConditionalCommand(
        new InstantCommand(intake::run, intake),
        new InstantCommand(intake::stop, intake),
        () -> {
            intake.toggle();
            return intake.active();
        }
    ));
```

An example of usage could be in Velocity Vortex, where the beacons were either red or blue. Using a color sensor, we can detect the color and then perform some action based on whether it was red or blue.

```java
// pseudocode for instantiating the command
ConditionalCommand pressBeacon = new ConditionalCommand(
    new InstantCommand(beaconPresser::pushRed, beaconPresser),
    new InstantCommand(beaconPresser::pushBlue, beaconPresser),
    () -> vision.output() == BeaconColor.RED
);

pressBeacon.schedule();    // schedule the command

/* This is also useful in sequential command groups */
```

As you can see, conditional commands are very useful for switching between states with a certain state. We will see later that we would want to use a `SelectCommand` when working with several states and not a simple command that switches between two.

### UninterruptibleCommand

Schedules a given command as uninterruptible. This command's parameter is a single command, so multiple commands need to be put in a CommandGroup first. See [#schedulecommand](#schedulecommand "mention") for scheduling commands as interruptible.

```java
// With one command:
UninterruptibleCommand uninterruptibleCommand = new UninterruptibleCommand(
    // Command
);


// With multiple commands:
UninterruptibleCommand uninterruptibleCommand = new UninterruptibleCommand(
    new SequentialCommandGroup(
        // Command, 
        // Command
    )
);
```

### ScheduleCommand

Does exactly as the name suggests: schedules commands (all as interruptible). You can input a variable number of command arguments to schedule, and the command will schedule them on initialization. After this, the command will finish. This is useful for forking off of command groups. See [#uninterruptiblecommand](#uninterruptiblecommand "mention") for scheduling commands as uninterruptible.

So far we've been using the convenience commands we've learned in tandem and how they can be used together to produce more efficient paradigm utility. This is no exception for the `ScheduleCommand`. We can use a conditional command to schedule a desired command.

```java
SequentialCommandGroup auto = new SequentialCommandGroup(
    ...,
    new ConditionalCommand(
        new ScheduleCommand(
            // schedule commands
        ),
        new ScheduleCommand(
            // schedule commands
        ),
        ...    // boolean supplier
    ),
    ...
);
```

It is important to note that the schedule command will finish immediately. Which means any following commands in the sequential command group will be run. The point of the schedule command is to fork off from the command group so that it can be run separately by the scheduler.

### SelectCommand

The select command is similar to a conditional command but for several commands. This is especially useful for a state machine. Let's take a look at the ring stack from the 2020-2021 Ultimate Goal season.

```java
public enum Height {
    ZERO, ONE, FOUR
}

public Height height() {
    // some code to detect height of the starter stack
}

...

SelectCommand wobbleCommand = new SelectCommand(
    // the first parameter is a map of commands
    new HashMap<Object, Command>() {{
        put(Height.ZERO, new PurePursuitCommand(...));
        put(Height.ONE, new PurePursuitCommand(...));
        put(Height.FOUR, new PurePursuitCommand(...));
    }},
    // the selector
    this::height
);
```

A select command is finished when the selected command also finishes. An alternative use of the select command is to run a command given a supplier instead of a map. Below is a comparable version to the one above:

```java
public enum Height {
    ZERO, ONE, FOUR
}

public Height height() {
    // some code to detect height of the starter stack
}

// returns a command for the wobble goal action
public Command wobbleCommand() {
    Height rings = this.height();
    switch (rings) {
        case ZERO:
            return ...;
        case ONE:
            return ...;
        case FOUR:
            return ...;
    }
}

...

SelectCommand wobbleCommand = new SelectCommand(
    // pass in a command supplier
    this::wobbleCommand
);
```

### PerpetualCommand

As opposed to an instant command, a perpetual command swallows a command and runs it perpetually i.e. it will continue to execute the command passed in as an as argument in the constructor and ignore that command's `isFinished` condition. It can only end if it is interrupted. This makes them useful for default commands, which are interrupted when another command requiring that subsystem is currently being run and is not scheduled again until that command ends.

Let's take a look back at the command bindings for when we learned `InstantCommand`, Instead of doing `whileHeld` and `whenReleased` binding, a more idiomatic method is to use a default command to stop the intake when the button is released instead (which cancels the command once the trigger/button is inactive, allowing the default command to be scheduled).

```java
/* in your opmode */

Motor intakeMotor = new Motor(hardwareMap, "intake");
Intake intake = new Intake(intakeMotor);

toolOp.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
    .whileHeld(new InstantCommand(intake::run, intake));

intake.setDefaultCommand(new PerpetualCommand(stopIntakeCommand));

// this isn't actually needed; really you'd do this:
intake.setDefaultCommand(new RunCommand(intake::stop, intake));
```

Note that a perpetual command adds all the requirements of the swallowed command.

### RepeatCommand

`RepeatCommand` composes (wraps) another `Command` and repeatedly restarts it each time it finishes, until one of the configured termination conditions occurs.

Compared to `PerpetualCommand` (which simply keeps executing the wrapped command while suppressing its natural completion), `RepeatCommand` lets the wrapped command complete, explicitly ends it, and immediately initializes it again for a fresh cycle. This is ideal for discrete, restartable “unit” actions that you want to chain, such as for autonomous TeleOp cycling. Although it does not extend `CommandGroupBase`, `RepeatCommand` is treated as a command group.

RepeatCommand has 3 different constructors as follows:

#### 1. Repeat until interrupted:

```java
new RepeatCommand(command);
```

Runs `command` forever (until another command preempts it or it is canceled).

#### 2. Repeat until condition:

```java
new RepeatCommand(command, () -> someCondition);
```

Effectively a repeat until loop, with the second parameter being a `BooleanSupplier` condition. In other words, it repeats `command` until `someCondition` is true.

#### 3. Repeat an integer amount of times

```java
new RepeatCommand(command, repeatTimes);
```

Repeats the wrapped command for the number given in the second parameter. As such, `repeatTimes` must be greater than 0 (passing 0 or a negative number throws an `IllegalArgumentException`).

### RetryCommand

`RetryCommand` is a command originating from [Marrow](https://skeleton-army.gitbook.io/marrow). Designed for SolversLib, it's been moved over to SolversLib natively for convenience. The documentation below for it is copied verbatim from Marrow:\
\
`RetryCommand` is designed for building adaptable and reliable command sequences. It executes a command, and, if the specified condition is not met upon completion, automatically retries up to the specified amount of times.

This is especially useful for actions that may fail on the first attempt, such as vision-based alignment, object grabbing, or precise mechanism positioning.

To use `RetryCommand`, you need to provide the constructor with:

* **Command to run** – the initial action you want to execute.
* **(Optional) Alternative command to run on retries** – lets you customize the retry behavior per attempt (e.g., switching to a vision-assisted command if the initial attempt fails).
* **Success condition** – a boolean supplier that checks whether the action was successful. If this condition returns `false`, the command will be retried.
* **Maximum number of retries** – defines the maximum number of times the command can be retried.

The Constructors are as follows:

#### 1. Basic RetryCommand (Same Command on Repeat)

```java
RetryCommand(Command command, BooleanSupplier successCondition, int maxRetries)

// Example
new RetryCommand(
    new GrabCommand(claw),   // Command to run.
    () -> claw.isGrabbed(),  // If this condition is false
    5                        // retry up to 5 times.
)
```

#### 2. Advanced RetryCommand (Different Command on Repeat)

```java
RetryCommand(Command command, Command retryCommand, BooleanSupplier successCondition, int maxRetries)

// Example
new RetryCommand(
    new GrabCommand(claw),                  // Command to run initially.
    new DetectAndGrabCommand(claw, vision), // Command to run on each retry.
    () -> claw.isGrabbed(),                 // If this condition is false
    5                                       // retry up to 5 times.
)
```

This repeats a command that isn't the original that fails.

Here’s an example autonomous that uses `RetryCommand` together with SolversLib’s command system:

```java
@Autonomous
public class MyAuto extends CommandOpMode {
    private ClawSubsystem clawSubsystem;
    private VisionSubsystem visionSubsystem;

    @Override
    public void initialize() {
        clawSubsystem = new ClawSubsystem(hardwareMap);
        visionSubsystem = new VisionSubsystem(hardwareMap);

        schedule(
            new SequentialCommandGroup(
                // First, try to grab. If unsuccessful, retry grabbing up to 3 times.
                new RetryCommand(
                    new GrabCommand(clawSubsystem),
                    () -> clawSubsystem.isHoldingGameElement(),
                    3
                ),
                
                // ---- OR ----
                
                // First, try to grab. If unsuccessful, try to detect and grab using vision up to 3 times.
                new RetryCommand(
                    new GrabCommand(clawSubsystem),
                    new DetectAndGrabCommand(clawSubsystem, visionSubsystem), 
                    () -> clawSubsystem.isHoldingGameElement(),
                    3
                )
            )
        );
    }
}
```

{% hint style="warning" %}
This is by no means a functional autonomous program, and is purely used as an example.
{% endhint %}

Check out the [Marrow Retries page](https://skeleton-army.gitbook.io/marrow/concepts/beyond-the-basics/retries) for more info.

### WaitCommand

A `WaitCommand` does nothing and finishes after a specified duration, given in milliseconds. It is one of the most common pieces of "glue" inside a `SequentialCommandGroup`: use it whenever a mechanism needs real time to physically move before the next command should start, such as waiting for a servo to reach its position.

```java
WaitCommand(long millis)
```

For example, in a scoring sequence:

```java
SequentialCommandGroup score = new SequentialCommandGroup(
    new InstantCommand(claw::open, claw),
    // give the claw servo time to physically open
    new WaitCommand(500),
    new InstantCommand(lift::retract, lift)
);
```

`WaitCommand` can also be subclassed to make a command with an internal `Timer` (the protected `m_timer` field). If you want to wait for a condition instead of a fixed amount of time, use the `WaitUntilCommand` below.

### WaitUntilCommand

A `WaitUntilCommand` is run until the boolean supplied returns true. This is useful for when you have forked off from a command group. Let's expand upon the example from the [`ScheduleCommand`](#schedulecommand) but with a single schedule command.

```java
SequentialCommandGroup auto = new SequentialCommandGroup(
    ...,
    // schedule the command that forks off from
    // the command group
    new ScheduleCommand(forkedCommand),
    // wait until that command is finished
    new WaitUntilCommand(forkedCommand::isFinished),
    ...    // more commands
);
```

The following commands in the auto command group are only run after that forked command is finished due to that `WaitUntilCommand`.

### StartEndCommand

The `StartEndCommand` is essentially an `InstantCommand` with a custom end function. It takes two `Runnable` parameters and an optional varargs of subsystems to require. The first parameter is run on initialize and the second is run on end.

### DeferredCommand

Usually, when you schedule a command (like `new IntakeCommand(intake)`), all of its dependencies, parameters, and logic is fixed at construction time (when the command is instantiated, not run).

Sometimes, you don't know what command you need until later. For example:

* You want to choose the command based on **sensor data**
* You want to choose the command based on the **most recent state of a variable**

The `DeferredCommand` waits until the command is initialized to decide which command to run. It takes a `Supplier<Command>` and a `List` of required `subsystems` as input.

```java

class Door extends SubsystemBase {
	private boolean isOpen = false;
	public static int DOOR_DELAY = 500;
	...,
	
	public Command setOpen(boolean newState){
		if (isOpen == newState) { 
			return new InstantCommand(); 
		}

		isOpen = newState;
		return new WaitCommand(DOOR_DELAY);
	}
}

schedule(
	new DeferredCommand(() -> door.setOpen(false), Arrays.asList(door))
)
```

In this example, since the initial value of `isOpen` is `false`, without the use of `DeferredCommand`, no matter what the current state of the door is, `door.setOpen(false)` would return an `InstantCommand`. By using `DeferredCommand`, you can make the command use the current state instead of the state when the command was instantiated.

Note that the example above is simple and can be handled by a [#conditionalcommand](#conditionalcommand "mention") more conveniently.

### CallbackCommand

`CallbackCommand` wraps a single command and lets you attach callbacks that fire the first time a condition becomes true while the wrapped command is running. On initialization it schedules the wrapped command separately (similar to `asProxy`), and it finishes once the wrapped command is no longer scheduled. Each callback fires at most once: after its condition first returns true, it is removed.

```java
CallbackCommand(T command)
```

`CallbackCommand<T extends Command>` is generic: the type parameter `T` is the type of the wrapped command, which gives the `whenSelf` callbacks typed access to it. Like the `UninterruptibleCommand`, the constructor expects a single command, so multiple commands need to be put in a command group first.

The callback methods all return the `CallbackCommand` itself for chaining:

* `when(BooleanSupplier condition, Runnable action)` — runs the `Runnable` the first time the condition is true.
* `when(BooleanSupplier condition, Command action)` — schedules the `Command` the first time the condition is true.
* `whenSelf(BooleanSupplier condition, Consumer<T> action)` — passes the wrapped command to the `Consumer` the first time the condition is true.
* `whenSelf(Predicate<T> condition, Runnable action)` — like `when`, but the condition is a `Predicate` that receives the wrapped command.
* `whenSelf(Predicate<T> condition, Command action)` — schedules the `Command` the first time the predicate is true.
* `whenSelf(Predicate<T> condition, Consumer<T> action)` — passes the wrapped command to the `Consumer` the first time the predicate is true.

```java
schedule(
    new CallbackCommand<>(new DriveForwardCommand(drive))
        // raise the lift once the robot has driven 24 inches
        .when(() -> drive.getDistance() > 24, new LiftUpCommand(lift))
        // rumble the gamepad when the drive command reports it is finished
        .whenSelf(cmd -> cmd.isFinished(), () -> gamepad1.rumble(250))
);
```

Note that the `CallbackCommand` does not inherit the wrapped command's requirements; call `addRequirements()` on it separately if needed.

Every command also has `when(BooleanSupplier, Runnable)` and `when(BooleanSupplier, Command)` decorators that wrap the command in a `CallbackCommand` for you:

```java
schedule(
    new DriveForwardCommand(drive)
        .when(() -> drive.getDistance() > 24, new LiftUpCommand(lift))
);
```

### LambdaCommand

`LambdaCommand` lets you build an entire command out of lambdas without creating a new class, analogous to lambda functions or closures. Every phase of the command (`initialize()`, `execute()`, `isFinished()`, `end()`) can be supplied either through a chainable builder or all at once through a constructor.

{% hint style="warning" %}
By default, `isFinished()` returns `true`, so a `LambdaCommand` without a `setIsFinished()` call runs `initialize()` and `execute()` once and then ends, just like an `InstantCommand`.
{% endhint %}

#### 1. Builder style

Each setter returns the `LambdaCommand` itself for chaining:

* `setInitialize(Runnable initialize)` — run once when the command is scheduled.
* `setExecute(Runnable execute)` — run repeatedly while the command is scheduled.
* `setIsFinished(BooleanSupplier isFinished)` — the command ends once this returns `true`.
* `setEnd(Consumer<Boolean> end)` — run once when the command ends; the `Boolean` is whether it was interrupted.
* `setEnd(Runnable end)` — an overload that ignores the interrupted flag.
* `setRunWhenDisabled(BooleanSupplier runWhenDisabled)` / `setRunWhenDisabled(boolean runWhenDisabled)` — whether the command may run while the robot is disabled.
* `setName(String name)`, `setSubsystem(String subsystem)`, and `addRequirements(Subsystem... requirements)` also return the `LambdaCommand` for chaining.

```java
Command driveForward = new LambdaCommand()
    .setInitialize(() -> drivetrain.resetEncoders())
    .setExecute(() -> drivetrain.drive(0.5))
    .setIsFinished(() -> drivetrain.getDistance() > 24)
    .setEnd(interrupted -> drivetrain.stop())
    .setName("DriveForward")
    .addRequirements(drivetrain);
```

#### 2. Functional style

```java
LambdaCommand(Runnable initialize,
              Runnable execute,
              BooleanSupplier isFinished,
              Consumer<Boolean> end,
              String name,
              BooleanSupplier runWhenDisabled)
```

```java
new LambdaCommand(
    () -> drivetrain.resetEncoders(),
    () -> drivetrain.drive(0.5),
    () -> drivetrain.getDistance() > 24,
    interrupted -> drivetrain.stop(),
    "DriveForward",
    () -> false
).addRequirements(drivetrain);
```

#### 3. From an existing command

The static factory `LambdaCommand.from(Command command)` copies the phases of an existing command into a new `LambdaCommand`, which is useful when you only need to change one part of the original:

```java
// the same as liftCommand, but with a custom finish condition
Command liftUntilStalled = LambdaCommand.from(liftCommand)
    .setIsFinished(() -> lift.isStalled());
```

Note that `from()` does not copy the original command's requirements, so re-add them with `addRequirements()` if needed.

### FunctionalCommand

The last framework command we will discuss is the `FunctionalCommand`. It is useful for doing an inline definition of a complex command instead of creating a new command subclass. Generally, it is better to write a class for anything past a certain complexity.

The `FunctionalCommand` takes four inputs and a variable amount of subsystems for adding requirements. The four inputs determine the initialization, execution, and end actions, followed by the boolean supplier that determines if it is finished.

A good use of this is inside of a sequential command group. Let's use the previous command group and add a functional command.

```java
SequentialCommandGroup auto = new SequentialCommandGroup(
    ...,
    new FunctionalCommand(
        // init actions
        driveSubsystem::resetEncoders,
        // execute actions
        () -> {
            /* list of actions */
            // remember run() returns void
        },
        // end actions
        interrupted -> driveSubsystem.stop(),
        // is finished supplier
        () -> {
            /* logic that returns a boolean */
        },
        driveSubsystem,
        ...    // any other required subsystems
    ),
    new ScheduleCommand(forkedCommand),
    new WaitUntilCommand(forkedCommand::isFinished),
    ...
);
```

## The Commands Utility Class

`Commands` is a utility class of static factory methods that construct the framework commands from this page with less boilerplate.

{% hint style="info" %}
`Commands` (plural) is not the `Command` interface. It is a `final` utility class that only builds and returns commands — it cannot be instantiated or implemented.
{% endhint %}

All of the factory methods below are `static` and return a `Command`:

#### Action commands

* `Commands.none()` — does nothing and finishes immediately (an `InstantCommand`).
* `Commands.idle(Subsystem... requirements)` — does nothing until interrupted.
* `Commands.runOnce(Runnable action, Subsystem... requirements)` — an `InstantCommand`.
* `Commands.run(Runnable action, Subsystem... requirements)` — a `RunCommand`.
* `Commands.startEnd(Runnable start, Runnable end, Subsystem... requirements)` — a `StartEndCommand`.
* `Commands.runEnd(Runnable run, Runnable end, Subsystem... requirements)` — runs an action every iteration until interrupted, then runs the end action.
* `Commands.startRun(Runnable start, Runnable run, Subsystem... requirements)` — runs an action once, then another action every iteration until interrupted.
* `Commands.print(String message)` — a `PrintCommand` that prints a message and finishes.

#### Idling commands

* `Commands.waitMillis(long millis)` — a `WaitCommand`.
* `Commands.waitUntil(BooleanSupplier condition)` — a `WaitUntilCommand`.

#### Selector commands

* `Commands.either(Command onTrue, Command onFalse, BooleanSupplier selector)` — a `ConditionalCommand`.
* `Commands.select(Map<Object, Command> commands, Supplier<Object> selector)` — a `SelectCommand`.
* `Commands.defer(Supplier<Command> supplier, List<Subsystem> requirements)` — a `DeferredCommand`.
* `Commands.deferredProxy(Supplier<Command> supplier)` — defers to the supplied command and runs it by proxy.

#### Command groups

* `Commands.sequence(Command... commands)` — a `SequentialCommandGroup`.
* `Commands.perpetuatingSequence(Command... commands)` — a sequence that restarts and runs perpetually once the last command ends.
* `Commands.parallel(Command... commands)` — a `ParallelCommandGroup`.
* `Commands.race(Command... commands)` — a `ParallelRaceGroup`.
* `Commands.deadline(Command deadline, Command... otherCommands)` — a `ParallelDeadlineGroup`.

Since every factory is static, whole routines can be written inline:

```java
schedule(
    Commands.sequence(
        Commands.runOnce(claw::grab, claw),
        Commands.waitMillis(300),
        Commands.parallel(
            Commands.runOnce(lift::raise, lift),
            Commands.print("Scoring!")
        )
    )
);
```

You can see the full class in the [Commands source on GitHub](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/Commands.java).

## Command Decorators

Decorators are methods that allow you to make command bindings and logic without having to create new commands.

### withTimeout

Returns a `ParallelRaceGroup` with a specified timeout in milliseconds.

```java
schedule(
    fooCommand.withTimeout(1000) // ends after 1000 milliseconds
);
```

### interruptOn

Returns a `ParallelRaceGroup` that ends once a specified condition is met.

```java
schedule(
    fooCommand.interruptOn(() -> {
        /* BOOLEAN SUPPLIER */
        return ...;
    })
);
```

### whenFinished

Returns a `SequentialCommandGroup` that runs a given Runnable after the calling command finishes.

```java
schedule(
    fooCommand.whenFinished(() -> {
        /* RUNNABLE */
    })
);
```

### beforeStarting

Returns a `SequentialCommandGroup` that runs a given Runnable before the calling command is initialized.

```java
schedule(
    fooCommand.beforeStarting(() -> {
        /* RUNNABLE */
    })
);
```

### beforeStarting (overloaded)

An overloaded method of [beforeStarting](#beforestarting) that takes a Command as a parameter instead of a Runnable

```java
schedule(
    fooCommand.beforeStarting(
        barCommand    // a Command, not a Runnable
    )
);
```

### andThen

Returns a `SequentialCommandGroup` that runs all the given commands in sequence after the calling command finishes.

```java
schedule(
    fooCommand.andThen(
        barCommand, bazCommand, ...
    )
);
```

### deadlineWith

Returns a `ParallelDeadlineGroup` with the calling command as the deadline to run in parallel with the given commands.

```java
schedule(
    // ends when fooCommand finishes
    fooCommand.deadlineWith(
        barCommand, bazCommand, ...
    )
);
```

### alongWith

Returns a `ParallelCommandGroup` that runs the given commands in parallel with the calling command.

```java
schedule(
    fooCommand.alongWith(
        barCommand, bazCommand, ...
    )
);
```

### raceWith

Returns a `ParallelRaceGroup` that runs all the commands in parallel until one of them finishes.

```java
schedule(
    // runs until one of the commands finishes
    fooCommand.raceWith(
        barCommand, bazCommand, ...
    )
);
```

### perpetually

Swallows the command into a `PerpetualCommand` and returns it.

```java
// returns a perpetual command
Command perpetual = fooCommand.perpetually();
```

### asProxy

Swallows the command into a `ProxyScheduleCommand` and returns it. This is similar to a `ScheduleCommand` except it ends when all the commands that it scheduled are finished rather than immediately.

```java
// returns a proxy schedule command
Command proxySchedule = fooCommand.asProxy();
```


# Old Commands

package com.seattlesolvers.solverslib.command.old

**Please note that the following is deprecated. Refer to the new commands if you want to make use of the new paradigm.**

SolversLib provides a Command-based OpMode to define your robot's `OpMode` routine. Instead of hard-coding instructions in your OpMode, you can leverage the reusability of the `CommandOpMode` structure to predefined `Command` routines and run them sequentially. You can also set timeouts for a `Command` to automatically terminate a running command.

## Subsystem

Defines an organized module on your robot. One such example is a linear slide lift powered by a motor connected to a spool, additionally using encoders or touch/magnetic sensors to act as limit switches, preventing the lift from exceeding its bounds. In this example, the motor, its encoder, and any sensors used would fall under a single "Lift" Subsystem.

A Subsystem has five lifecycle phases:

* `initialize()`: Prepares the physical hardware for activation of the Subsystem. This phase is intended to be used for hardware map initialization, zeroing of encoders (as required), and gathering any initially required sensor data.
* `reset()`: Returns the Subsystem back to its original state. This phase is distinct from `initialize()` as it is expected that the hardware map is already initialized. This phase is intended to return Subsystem hardware to its original condition and clear saved data.
* `loop()`: This is the main lifecycle phase of a Subsystem. This is where [Commands ](#command)could be issued to the Subsystem, or user/sensor input is used to operate the Subsystem. This phase is intended to repeatedly loop until `stop()` is called.
* `stop()`: Halts all action of the Subsystem, bringing all hardware devices to stop. It is recommended to set the Zero Power Behavior of Motors to `BRAKE` in this phase, as motion is designed to entirely cease. A Subsystem should be designed to enable a `reset()` from this phase to return it to normal operation.
* `disable()`: Deactivates the Subsystem, rendering it unusable until the next `initialize()`. It is recommended to set the Zero Power Behavior of Motors to `FLOAT` in this phase, as Subsystem will no longer be receiving input. A Subsystem should *NOT* be designed to enable `reset()`, `initialize()` is required.

## Command

Defines a single, executable command. A Command defines the actions of multiples parts (or [`Subsystem`](#subsystem)) of the robot to take at once. A Command has three lifecycle phases:

* `initialize()`: The initial subroutine of a Command. Called once when the Command is initially scheduled.
* `execute()`: The main body of a Command. Called repeatedly while the Command is scheduled.
* `end()`: The action to take once the Command is completed. Called once on the Command's completion.

`Command` also provides a `isFinished()` method which returns `true` if the Command has completed and `false` otherwise. Note: `end()` will always run after the Command is unscheduled. You do not need to check if the Command `isFinished()` to run your own `end()` method.

## CommandOpMode

Defines an `OpMode` which is designed to run on [Commands](#command) as opposed to manual method calls. Before using a CommandOpMode, Commands and [Subsystems](#subsystem) need to be defined. CommandOpMode runs an internal `ElapsedTimer` which ensures Commands terminate after their specified timeouts.

Although CommandOpMode extends `LinearOpMode` , it is not required to use any of the methods provided explicitly. These methods are all called internally by `runOpMode()` (it is not necessary to override this method within your own extension of CommandOpMode) within the three lifecycle phases of a CommandOpMode (it *is* required to override these methods):

* `initialize()`: Sets up and calls `initialize()` of all attached Subsystems and Commands. It is vital to setup Subsystems before their Commands, as doing the reverse could likely raise NullPointerExceptions.
* `initLoop()`: Called repeatedly after `initialize()` but before the user presses "Play" on the Driver Station
* `run()`: The main loop of a CommandOpMode. Called immediately after the user presses the "Play" button on the Driver Station.

Internally, these three lifecycle phases are tied together under an override of the `runOpMode()` method of `LinearOpMode`:

```java
@Override
public void runOpMode() throws InterruptedException {
    commandTimer = new ElapsedTime();
    initialize();
    while(!isStopRequested() && !isStarted()) { // Runs after "Init" 
        initLoop();                             // but before "Play"
    }
    run();
}
```

Once the user defines Commands, Subsystems, and the three lifecycle phases, the user can add Commands to the workflow using `addSequential()`. `addSequential()` adds a Command to be run within a certain specified timeout. The user can also optionally set a custom loop interval time (which defaults to 20 ms). `addSequential()` first initializes its passed-in Command, runs it every 20 ms (terminating if it reaches its timeout), and checks if the Command `isFinished()`. If true, the Command will exit the loop and run its `end()` lifecycle method.


# EasyOpenCV

Deprecated

FTCLib's vision was extremely outdated and therefore is now deprecated and should not be used.

If you are interested in using vision, please look at the far superior [EasyOpenCV](https://github.com/OpenFTC/EasyOpenCV) instead.


# Support SolversLib

## Support SolversLib

SolversLib is solely community-driven and currently is not the standard tool for programming in FTC. In order to get to that point, we need to garner a large audience. If you have worked with our library, consider using some of our branding tools that you can find under the [brand package](https://github.com/FTC-23511/SolversLib/tree/master/brand) in our repository. Spreading news by word of mouth also works and make sure to point them towards here.

We would also greatly appreciate it if you would contribute to SolversLib. Make sure to read our [contributing page](https://github.com/FTC-23511/SolversLib/blob/master/.github/CONTRIBUTING.md) on the GitHub for information on how to contribute. We are always willing to accept new pull requests. In fact, SolversLib is built on community contributions and would not have been possible if not for our contributors.

\
If you really want to donate money, you can donate money to our team, FTC #23511 at our [donate page](https://hcb.hackclub.com/donations/start/ftc23511) (we are a nonprofit).


# Additional Reading

We suggest you also read some other FTC materials as well, especially if you are a rookie team or just not all that knowledgeable in regards to FTC programming.

The first resource we recommend you reading is [Game Manual 0](https://gm0.copperforge.cc/en/stable/), or gm0 for short. They have a section on software you can find [here](https://gm0.copperforge.cc/en/stable/docs/software/index.html). gm0 offers a great amount of support for teams who are just starting and would like to learn more concepts that have been put together by people from other teams.

Please be sure to check out other libraries that have inspired us, like [RoadRunner](https://rr.brott.dev/), [Pedro Pathing](https://pedropathing.com) (which SolversLib supports) and [EasyOpenCV](https://github.com/OpenFTC/EasyOpenCV). You can use these libraries in conjunction with SolversLib if you would like.


# Welcome

SolversLib an updated and maintained fork of the older FTCLib. PRs made to both SolversLib & FTCLib are regularly merged into SolversLib. The changes made to SolversLib were designed to not break existing FTCLib code (with the exception of FTCLib's [vision](broken://pages/rZtsaB1AYEc7Mzde5fFA) module), allowing you to build upon preexisting FTCLib codebases.

FTCLib was initially meant to be a port of WPILib, which is the standard programming library for FRC that almost all teams use. However, with FTC, there are a ton of libraries that not many people have heard about, especially rookie teams who are just starting. The goal of FTCLib is to improve the initial programming experience for new members as well as greatly enhance the efficiency of code for veterans.

We support the transition of teams from programming systems like Blocks and OnBot Java to Android Studio. One of our goals is to make this transition easier for you if you have not already.

Please read the [installation](/0.3.4/installation) instructions before getting started with anything!

### SolversLib Leads:

* Arush Y (Owner) - [FTC 23511](https://www.seattlesolvers.com/)
* Saket T - [FTC 23511](https://www.seattlesolvers.com/)

### SolversLib Contributors:

* Oscar C - [Dairy Foundation](https://github.com/Dairy-Foundation)
  * Special thanks to Oscar for hosting SolversLib on the Dairy Foundation!
* Noam - [FTC 23644](https://www.youtube.com/@SkeletonArmy23644)
* Daniel - FTC 7854
* Nv7 - [FTC 21229](https://www.stem-bridge.org/home)
* Lucas B - FTC 15215, 22407, and 24736
* Suchir R - FTC [19448](https://www.instagram.com/xbots_ftc/)
* Kully - [KoalaLog](https://github.com/ori-coval)

### Docs Contributors:

* Arush Y (Owner) - [FTC 23511](https://www.seattlesolvers.com/)
* Dohun - [FTC 22489](https://www.youtube.com/@GNCEDiamond22489)
* Rowan M - [FTC 3805](https://atomic.ehrobotics.org/pages/team/)

### Contact:

* [SolversLib Discord](https://discord.com/invite/dJbSWcXsUp)
* [Email](mailto:software@seattlesolvers.com)
* [GitHub](https://github.com/FTC-23511/SolversLib)


# Changelog

This is the Changelog for SolversLib versions for 0.3.1 and higher. It includes the changes made from the previous iteration, and important notes for it as well. You can change SolversLib documentation versions by using the selector in the top left corner.

## [0.3.4](/0.3.4)

#### Core:

* Fixed bug with `RunMode` in `MotorEx` using distance instead of position
* Added `ServoExGroup`
* Added optional `GamepadEx` Slew Rate Limiters (mostly for swerve, but can be used for mecanum chassis as well)
* Added optional `P2PController` Slew Rate Limiters (for swerve)
* Added `mirror()` method to `Pose2d for` alliance swapping
* Updated swerve kinematics with maxSpeed
* Refactored `Timing` class
  * `Stopwatch`
  * `Timer`
  * `Rate` (Refresh Rate Timer)
* Added `rawPower` tracking and access in hardware classes
* Bug fixed `MotorEx` 's PositionControl using `encoder.getPosition()` instead of `encoder.getDistance()`&#x20;
* Added `LambdaCommand`
* Added `SubsystemBase` `isAvailable()` method
* Added CommandScheduler `getScheduledCommands()` method
* Added `Commands` utility class for pre-defined actions (not to be confused with the Command interface)
* Added `CallbackCommand`
* Bug fixed deadline `end()` command in `ParallelDeadlineGroup`

#### PedroPathing:

* Fixed maxPower not saving for multiple paths

#### Photon (NEW):

* Added stable PhotonCore for FTC

## [0.3.3](/0.3.4)

#### Core:

* Rewrote Hardware classes:
  * Deprecated [`ServoEx` interface](/0.3.4/features/hardware/motors-1#servoex-interface-and-simpleservo-class) and [`SimpleServo` class](/0.3.4/features/hardware/motors-1#servoex-interface-and-simpleservo-class)
    * The new [`ServoEx` class](/0.3.4/features/hardware/servos#servoex) serves as a replacement to both of the classes above.
  * Added [`CRServoEx`](/0.3.4/features/hardware/servos#crservoex-and-absoluteanalogencoder) and [`AnalogAbsoluteEncoder`](/0.3.4/features/hardware/servos#crservoex-and-absoluteanalogencoder) (4th wire support on Servos)
  * Added optional power caching to `Ex` classes like `MotorEx`, `ServoEx`, and `CRServoEx`, etc.
  * Added a Color Sensor class for the [`REV Color Sensor V3`](/0.3.4/features/hardware/sensors#sensors)
  * Modified [`SensorDistance`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistance.java) & [`SensorDistanceEx`](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistanceEx.java) to handle custom minimum and maximum distance threshold values
* Beta Swerve support:
  * Added [`Point-to-Point`](/0.3.4/pathing/point-to-point) control
  * Rewrote Swerve kinematics and updated `ChassisSpeeds`
  * Created [`SolversLib Visualizer`](https://visualizer.seattlesolvers.com/) (forked from Pedro Pathing Visualizer)
* Implemented abstract [`Controller`](/0.3.4/features/controllers#using-the-controller-class) class and added [`SquIDF`](/0.3.4/features/controllers/squidf) controller
* Added native [`RetryCommand`](/0.3.4/command-base/command-system/convenience-commands#retrycommand) from Marrow

#### PedroPathing:

* Added two new Pedro Commands: [`TurnToCommand`](/0.3.4/pedro-pathing/pedro-commands/hold-point-command-2) and [`TurnCommand`](/0.3.4/pedro-pathing/pedro-commands/hold-point-command-1)&#x20;
* Added support for Pedro Pathing versions 2.0.0 and higher

## [0.3.2](/0.3.2)

#### Core:

* Added beta SquID support and early Swerve Kinematics
* Fixed `CommandScheduler`'s `cancelAll()` from throwing an error
* Added additional constructors to [`RepeatCommand`](/0.3.4/command-base/command-system/convenience-commands#repeatcommand)
* Rewrote SolversHardware wrappers into hardware

#### PedroPathing:

* Added new Pedro Command: [`HoldPointCommand`](/0.3.4/pedro-pathing/pedro-commands/hold-point-command)
* Added support for Pedro Pathing 1.0.9

## [0.3.1](/0.3.1)

#### Core:

* Add SolversHardware caching wrappers
* Fixed known FTCLib bugs/issues:
  * [Double ending issue in ParallelRaceGroup](https://github.com/FTCLib/FTCLib/pull/261)
  * [SequentialCommandGroup is incompatible with other command groups](https://github.com/FTCLib/FTCLib/issues/253)
  * [ConcurrentModificationException in cancelAll()](https://github.com/FTCLib/FTCLib/pull/260)
  * [IllegalAccessError when trying to build clean dev branch on Windows](https://github.com/FTCLib/FTCLib/issues/255)
  * [Missing Aliases for PS4 and PS5 buttons](https://github.com/FTCLib/FTCLib/issues/250)

#### Pedro Pathing:

* Added new Pedro Command: [`FollowPathCommand`](/0.3.4/pedro-pathing/pedro-commands/followpathcommand)
* Added support for Pedro Pathing 1.0.8


# Installation

How to import SolversLib into your Android Studio FTC Project

## Option 1: Manually Install

This works with or without having [FTCLib](https://ftclib.org/) already installed. SolversLib allows you to easily migrate from FTCLib to SolversLib while keeping all of your code, or install normally.

### build.gradle

The first thing you need to change from FTCLib is the dependency in `build.gradle`

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
dependencies {
    // implementation "org.ftclib.ftclib:core:2.1.1" remove FTCLib core
    implementation "org.solverslib:core:0.3.4" // core
```

{% endcode %}

Or, if you are using [Pedro Pathing](/0.3.4/pedro-pathing/pedro-commands), change to this dependency block

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
dependencies {
    // implementation "org.ftclib.ftclib:core:2.1.1" remove FTCLib core
    implementation "org.solverslib:core:0.3.4" // core
    implementation "org.solverslib:pedroPathing:0.3.4" // pedroPathing
}
```

{% endcode %}

And if you are using [Photon](/0.3.4/photon/what-is-photon), change to this dependency block

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
dependencies {
    // implementation "org.ftclib.ftclib:core:2.1.1" remove FTCLib core
    implementation "org.solverslib:core:0.3.4" // core
    implementation "org.solverslib:pedroPathing:0.3.4" // pedroPathing
    implementation "org.solverslib:photon:0.3.4" // photon
}
```

{% endcode %}

{% hint style="danger" %}
**Important:** Different versions of SolversLib correlate with different versions of Pedro Pathing.

[SolversLib 0.3.3](/0.3.4/installation)+ is for Pedro Pathing 2.0.0 and higher\
[SolversLib 0.3.2](/0.3.2/installation) is for Pedro Pathing 1.0.9\
[SolversLib 0.3.1](/0.3.1/installation) is for Pedro Pathing 1.0.8
{% endhint %}

The latest version numbers (as well as a list of all version numbers) are available at:

* Latest `core` version: <https://repo.dairy.foundation/#/releases/org/solverslib/core>
* Latest `pedroPathing` version: <https://repo.dairy.foundation/#/releases/org/solverslib/pedroPathing>
* Latest `photon` version: <https://repo.dairy.foundation/#/releases/org/solverslib/photon>

{% hint style="danger" %}
**Warning:** If you choose to use the Pedro Pathing module, you still need to [install Pedro Pathing 2.0.0](https://pedropathing.com/)+ in order to use it.
{% endhint %}

Please note that you should not and cannot have both FTCLib and SolversLib installed at the same time.

### Repositories (required)

Finally in your repositories block, add the following code. You may have other content here, especially if you have the Pedro Pathing library installed. If you do not have a repositories block, you can add it above your dependencies block.

{% code title="build.gradle (Module: TeamCode)" %}

```groovy
repositories {
    maven {
        url "https://repo.dairy.foundation/releases"
    }
}
```

{% endcode %}

### Changing Imports (Only if Migrating from FTCLib)

Because the package names will be different, you can either manually replace all instances of `com.arcrobotics.ftclib` with `com.seattlesolvers.solverslib` , or use a command in a terminal to replace them all at once for you. Please make sure you either open a terminal into your Android Studio project or use the built-in Android Studio terminal to run the commands below.

**FTCLib Imports to SolversLib Imports (MacOS/Linux):**

```bash
find . -type f -name "*.java" -exec sed -i '' 's/com.arcrobotics.ftclib/com.seattlesolvers.solverslib/g' {} +
```

**SolversLib Imports to FTCLib Imports (MacOS/Linux):**

```bash
find . -type f -name "*.java" -exec sed -i '' 's/com.seattlesolvers.solverslib/com.arcrobotics.ftclib/g' {} +
```

**FTCLib Imports to SolversLib Imports  (Windows):**

```powershell
Get-ChildItem -Recurse -Filter *.java | ForEach-Object { 
    (Get-Content $_.FullName) -replace 'com.arcrobotics.ftclib', 'com.seattlesolvers.solverslib' | 
    Set-Content $_.FullName 
}
```

**SolversLib Imports to FTCLib Imports (Windows):**

```powershell
Get-ChildItem -Recurse -Filter *.java | ForEach-Object { 
    (Get-Content $_.FullName) -replace 'com.seattlesolvers.solverslib', 'com.arcrobotics.ftclib' | 
    Set-Content $_.FullName 
}
```

### Sync Gradle and Finished!

![Click that button and if successful, you can now use SolversLib](/files/xUgmRLRJnya2Afvv6Bcr)

## Option 2: Installing from SolversLib Quickstart

An alternative option is to simply use the SolversLib Quickstart. Similar to the FTCLib Quickstart, SolversLib has a Quickstart with this library fully set up. You can view it at <https://github.com/FTC-23511/SolversLib-Quickstart>. You can either fork or clone this repository as needed to use it.&#x20;

In addition, the Quickstart also has the Pedro Pathing library installed and added along with the SolversLib `pedroPathing`\` dependency, meaning that it is hassle-free. If you don't want the Pedro Pathing part, you can simply delete the relevant files and dependencies.


# Javadocs

Javadocs are automatically created documentation for Java classes. It gives a basic decription on how to use those methods and classes, although a more in-depth explanation can be found within this documentation GitBook itself.

Below are the Javadocs for each module:

* `core`: <https://repo.dairy.foundation/javadoc/releases/org/solverslib/core/latest>&#x20;
* `pedroPathing`: <https://repo.dairy.foundation/javadoc/releases/org/solverslib/pedroPathing/latest>
* `photon`: <https://repo.dairy.foundation/javadoc/releases/org/solverslib/pedroPathing/latest>

You can replace `latest` with your desired version number to get Javadocs for that version.

{% hint style="info" %}
There are also auto Javadocs for beta versions/snapshots in addition to&#x20;
{% endhint %}


# Hardware

package com.seattlesolvers.solverslib.hardware

Each hardware device in SolversLib is based on the `HardwareDevice` interface. This comes with two methods inherited by every device:

* `disable()`: disables the device
* `getDeviceType()`: returns a String characterization of the device

Optional power caching has also been added to the `Ex` type classes as well to help further loop times as well.

SolversLib offers *a lot* of hardware devices that can be implemented or customized into your program. The best advice we can give to users is to take a look at the subpages in this catogery ([Servos](/0.3.4/features/hardware/servos), [Motors](/0.3.4/features/hardware/motors), and [Sensors](/0.3.4/features/hardware/sensors)) and the [hardware package](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/hardware) in the SolversLib repository.


# Servos

package com.seattlesolvers.solverslib.hardware

## ServoEx

The [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) class allows for more methods and actions than the normal servo class in the SDK. You can change the position of the servo relative to the last position or set it to an absolute position. You can either specify a position within the range of the servo's motion or have it rotate a certain number of specified angle units.

It serves as a successor to the [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) interface and [SimpleServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SimpleServo.java) class, which have now been Deprecated. A copy of the original ServoEx interface documentation can be viewed [here](/0.3.4/features/hardware/motors-1#servoex-interface-and-simpleservo-class).

### Constructors:

#### 1. Regular (no Angle Control)

```java
ServoEx(HardwareMap hwMap, String id)

ServoEx servoEx = new ServoEx(hardwareMap, "servoEx");
```

#### 2. Angle Control (with Min and Max angle)

```java
ServoEx(HardwareMap hwMap, String id, double min, double max)
```

`MIN_ANGLE` and `MAX_ANGLE` are the minimum and maximum angle positions in degrees you would like to set the servo. This functionally serves as the servo's effective range. If you want to change the effective range at any point, you can do the following:

```java
// change the effective range to a min and max in DEGREES
servo.setRange(MIN_ANGLE, MAX_ANGLE);

// change the range to a min and max in RADIANS
servo.setRange(MIN_ANGLE, MAX_ANGLE, AngleUnit.RADIANS);

// return the effective range
double degreeRange = servo.getAngleRange();

// return the effective range in RADIANS
degreeRange = servo.getAngleRange(AngleUnit.RADIANS);
```

#### 3. Angle Control (with Range and AngleUnit)

```java
ServoEx(HardwareMap hwMap, String id, double range, AngleUnit angleUnit)
```

This is similar to the 2nd constructor, except that you specify the angular range (from when the servo is set to 0 to 1). As such, you also need to define the AngleUnit (Degrees or Radians) for that range.

### Utility Methods:

You can use `setInverted()` invert the servo's direction as well:

```java
// invert the servo
servo.setInverted(true);

// get if the servo is inverted (true if inverted, false if not)
boolean isInverted = servo.getInverted();
```

To turn to positions and angles, utilize the following methods:

* `rotateByAngle`: turns the servo a number of angle units relative to the current angle
* `turnToAngle`: sets the absolute angle of the servo
* `rotateBy`: turns the servo a relative positional distance from the current position
* `setPosition`: set the absolute position of the servo (from 0 to 1)

### Power Caching

Additionally, as an `Ex` class, `ServoEx` supports caching. If the power set to that hardware is less than an adjustable threshold, it will not send a write to it to help with loop speeds. The default threshold, which is called `cachingTolerance` , is 0.0001.

You can use `.setCachingTolerance` to adjust `cachingTolerance` it as needed.

```java
ServoEx servoEx = new ServoEx(hardwareMap, "servoEx");

servoEx.setCachingTolerance(0.0001);
```

{% hint style="warning" %}
This default value of `0.0001`  effectively disables the feature. To actually see results with power caching, increase it to `0.01` or higher depending on the precision required.
{% endhint %}

## CRServo

The [CRServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServo.java) class is just a motor object intended to be used for a continuous rotation servo. Its general purpose is to be used in SolversLib classes that require a `Motor` input. It works just like a regular motor, without any of the encoder stuff. As such, it extends the `Motor` class, and can be used in a [CRServoGroup](#crservogroup).

```java
CRServo crServo = CRServo(hardwareMap, "CRServo");

crServo.set(0.5);
```

## CRServoEx & AbsoluteAnalogEncoder

The `CRServoEx` class is an advanced wrapper for continuous rotation servos (CRServos), adding key features for enhanced control and integration, including:

* **Absolute analog encoder support** (e.g., for Axon servos)
* **Optimized positional control using PIDF** (required for absolute encoder)
* **Power caching for improved loop performance**
* **Custom PWM range support**

It it extends `CRServo`, which in turn extends the `Motor` class, and can also be used in a [CRServoGroup](#crservogroup).

The `AbsoluteAnalogEncoder` class is an advanced wrapper for Analog input AnalogInput absolute encoders, which are most commonly seen on servos with a 4th wire (like Axon Servos). It is best used in conjuction with `CRServoEx` .

### Constructors (AbsoluteAnalogEncoder)

#### 1. Basic AbsoluteAnalogEncoder

```java
AbsoluteAnalogEncoder(HardwareMap hwMap, String id)

// Example
AbsoluteAnalogEncoder encoder = new AbsoluteAnalogEncoder(hardwareMap, "absoluteEncoder");
```

This defaults to a range of 3.3, and an `AngleUnit` of Radians. If you do not know what you need, this is most likely it (and is functional for Axon servos).

#### 2. Advanced CRServo

```java
AbsoluteAnalogEncoder(HardwareMap hwMap, String id, double range, AngleUnit angleUnit)

// Example
AbsoluteAnalogEncoder absoluteEncoder = new AbsoluteAnalogEncoder(hwMap, "encoder", 3.3, AngleUnit.RADIANS)
```

This allows you to set your own range and An`gleUnit` if necessary. These constructors are used automatically in the designated CRServoEx constructors below as well.

* `angleUnit`: Angle unit for moving servo to position (`AngleUnit.DEGREES` or `AngleUnit.RADIANS`)
* `analogRange`: Voltage range for the encoder (e.g., 3.3V or 5V, depending on hardware)
  * **Default: 3.3V**
* `encoderID`: Name of the absolute encoder in hardware map

### Constructors (CRServoEx)

#### 1. Basic CRServo (Raw Power)

```java
CRServoEx(HardwareMap hwMap, String id)
```

* No encoder required; runs in `RawPower` mode by default. This is similar to `CRServo` , with a few extra features.

#### 2. Positional Control with Encoder

```java
CRServoEx(HardwareMap hwMap, String id, AbsoluteAnalogEncoder absoluteEncoder, RunMode runmode)

// Example
CRServoEx crServoEx = new CRServoEx(hardwareMap, "crServoEx", encoder, CRServoEx.RunMode.OptimizedPositionalControl)

```

* `hwMap`: FTC HardwareMap instance
* `id`: Configuration name of the CRServo
* `absoluteEncoder`: Instance of `AbsoluteAnalogEncoder` (must be initialized separately)
* `runmode`: Mode to run (see below)

{% hint style="danger" %}
**Warning:** If you have set the runmode to `OptimizedPositionalControl`, regardless if you want angle-based control, you must use a valid `AbsoluteAnalogEncoder` and also set the `PIDF` (see below for more information). Failing to do will result in an error being thrown.
{% endhint %}

#### 3. Advanced Encoder Configuration

```java
CRServoEx(HardwareMap hwMap, String id, String encoderID, double analogRange, AngleUnit angleUnit, RunMode runmode)
```

Instead of passing an `AbsoluteAnalogEncoder` in, this overloaded constructor has parameters to create one inside of the instance of the `CRServoEx`.

* Allows specifying encoder parameters:
  * `encoderID`: Name of the absolute encoder in hardware map
  * `analogRange`: Voltage range for the encoder (e.g., 3.3V or 5V, depending on hardware)
    * **Default: 3.3V**
  * `angleUnit`: Angle unit for moving servo to position (`AngleUnit.DEGREES` or `AngleUnit.RADIANS`)
    * **Default**: If not specified, defaults to `AngleUnit.RADIANS` unless overridden
  * `runmode`: See below

### Using a CRServoEx RunMode

Like the `Motor`'s  RunMode, the CRServoEx RunMode is a method of running the CRServoEx when power is supplied. However, there are only two modes: `OptimizedPositionalControl`, and `RawPower`.

```java
// in CRServoEx.java

/**
 * The mode in which the CR servo should behave.
 */
public enum RunMode {
    /**
     * Mode in which the CR servo takes the shortest path to reach a specific angle
     * Requires PIDF tuning (see below) + absolute encoder
     */
    OptimizedPositionalControl,
    /**
     * Default mode in which the CR servo is controlled with raw power
     */
    RawPower
}
```

#### `OptimizedPositionalControl`

* **Requirements**: An absolute encoder and PIDF coefficients.
* **Behavior**: When you call `.set(angle)`, the servo will move to the target angle using the shortest path.
  * Example:

    ```java

    AbsoluteAnalogEncoder encoder = new AbsoluteAnalogEncoder(hardwareMap, "absoluteEncoder");
    CRServoEx crServoEx = new CRServoEx(hardwareMap, "crServoEx", encoder, CRServoEx.RunMode.OptimizedPositionalControl)

    crServoEx.setPIDF(new PIDFCoefficients(0.001, 0.0, 0.1, 0.0001));
    crServoEx.set(Math.toRadians(90)); // move to 90 degrees (in radians)
    ```
  * If PIDF not set, positional control will throw an error.

#### `RawPowerMode`

* Acts like a normal CRServo's `RawPower`.

### PIDF Control

For positional control, you **must** set PIDF coefficients:

```java
s_crServoEx.setPIDF(new PIDFCoefficients(double P, double I, double D, double F));
```

* These coefficients will be used by the internal PIDF controller to compute the necessary power to reach the target angle smoothly and quickly.

{% hint style="warning" %}
**Important:** These PIDF coeffecients are used in SolversLib's PIDF class for the calculations. As such, you should tune it as if you were to tune a normal PIDF.
{% endhint %}

### Power Caching

Finally, like `MotorEx` , `CRServoEx` supports power caching. If the power set to that hardware is less than an adjustable threshold, it will not send a write to it to help with loop speeds. The default threshold, which is called `cachingTolerance` , is 0.0001.

You can use `.setCachingTolerance` to adjust `cachingTolerance` it as needed.

```java
CRServoEx s_crServoEx = new CRServoEx(hardwareMap, "s_crServoEx");

s_crServoEx.setCachingTolerance(0.0001);
```

### Example: Full Setup

```java
AbsoluteAnalogEncoder encoder = new AbsoluteAnalogEncoder(hardwareMap, "absoluteEncoder");
CRServoEx crServoEx = new CRServoEx(hardwareMap, "s_crServoEx", encoder, CRServoEx.RunMode.OptimizedPositionalControl);
crServoEx.setPIDF(new PIDFCoefficients(0.8, 0.02, 0.1, 0.0));
crServoEx.setCachingTolerance(0.0002);

crServoEx.set(Math.toRadians(135));
```

### Additional Methods

* `.setRunMode(RunMode runmode)`: Change runmode after construction.
* `.setAbsoluteEncoder(AbsoluteAnalogEncoder encoder)`: Switch encoder instance.
* `.getAbsoluteEncoder()`: Get associated encoder.
* `.getCachingTolerance()`: Retrieve current tolerance.
* `.getController()`: Get extended controller instance.
* `.getServo()`: Get underlying SDK CRServo object.
* `.getDeviceType()`: Returns device type (string).
* `.setPwm`: Sets the PWM range for the servo using

For more details, refer to the Javadocs within `CRServoEx.java`. The class supports method chaining for convenient setup and configuration.

## CRServoGroup

The CRServoGroup is like a MotorGroup, but for CRServo/CRServoEx. A CRServo group object takes several CRServos and runs them in parallel like a single CRServo. CRServo groups have one leader and a set of followers. For any group, there *must* be a leader, but the number of followers can be zero. This makes creating different drive profiles simpler. The constructor for a `CRServoGroup` is as follows:

```java
CRServoEx myCRServos = new CRServoGroup(leader, follower1, follower2, ...);
```

The number of followers is variable. The other methods of the `CRServoGroup` are the same as the ones found in `CRServo`. You can very simply treat a `CRServoGroup` object like a single `CRServo` object. The [flywheel sample](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/FlywheelSample.java) in the examples folder shows a few other methods you can utilize with the `MotorGroup`.

{% hint style="danger" %}
**Warning:** CRServo/CRServoEx should use a CRServoGroup (this) instead of a [MotorGroup](/0.3.4/features/hardware/motors#motorgroup)
{% endhint %}


# Motors

package com.seattlesolvers.solverslib.hardware.motors

SolversLib offers fully-featured motor wrappers for the ease of the user. Behind the scenes, it utilizes the advanced features of SolversLib to produce a more powerful implementation of the DcMotor objects offered in the SDK. Similarly, SolversLib has a `Motor` and `MotorEx` object, each of which allow for the user to directly access the instance object from the hardware map in the case of wanting to work with it directly.

## Creating a Motor Object

Creating a motor is as simple as passing in the hardware map, the name of the device in the robot controller config, and an optional third parameter of a GoBILDA motor type. This is necessary because the goBILDA motors in the configuration don't specify the different max RPM (rotations per minute) and CPR (counts per revolution).

There is also an option of using a custom CPR and RPM value.

```java
Motor m_motor_1 = new Motor(hardwareMap, "motorOne");
Motor m_motor_2 = new Motor(hardwareMap, "motorTwo", GoBILDA.RPM_312);
Motor m_motor_3 = new Motor(hardwareMap, "motorThree", CPR, RPM);

// grab the internal DcMotor object
DcMotor motorOne = m_motor_1.motor;
```

### Using a RunMode

A RunMode is a method of running the motor when power is supplied. There are three modes: `VelocityControl`, `PositionControl`, and `RawPower`.

```java
// in Motor.java

/**
 * The RunMode of the motor.
 */
public enum RunMode {
    VelocityControl, PositionControl, RawPower
}
```

Raw power sets the motor power directly through a value of $$\[-1, 1]$$ where the value represents the percentage of its maximum speed. This is also open loop control, which means there is no feedback control. This is the default mode of the motor.

```java
// set the run mode
m_motor.setRunMode(Motor.RunMode.RawPower);

// set the proportional output power of the motor
m_motor.set(0.37);    // 37% of maximum speed in current direction
```

Position control has the motor run to a desired position based on the input speed and the distance between current motor position and target position (in counts). This utilizes a P controller whose coefficient can be changed using `setPositionCoefficient(double)`. This is a tuned value. For tuning, we currently recommend using [FTC Dashboard](https://acmerobotics.github.io/ftc-dashboard/basics).

```java
// set the run mode
m_motor.setRunMode(Motor.RunMode.PositionControl);

// set and get the position coefficient
m_motor.setPositionCoefficient(0.05);
double kP = m_motor.getPositionCoefficient();

// set the target position
m_motor.setTargetPosition(1200);      // an integer representing
                                      // desired tick count

m_motor.set(0);

// set the tolerance
m_motor.setPositionTolerance(13.6);   // allowed maximum error

// perform the control loop
while (!m_motor.atTargetPosition()) {
  m_motor.set(0.75);
}
m_motor.stopMotor(); // stop the motor

/* ALTERNATIVE TARGET DISTANCE */

// configure a distance per pulse,
// which is the distance traveled in a single tick
// dpp = distance traveled in one rotation / CPR
m_motor.setDistancePerPulse(0.015);

// set the target
m_motor.setTargetDistance(18.0);

// this must be called in a control loop
m_motor.set(0.5); // mode must be PositionControl
```

Velocity control has the motor run using velocity in ticks per second with both a feedback and feedforward controller rather than simply setting the speed to a percentage of the maximum output speed. This can lead to smoother control for your motors, and is highly recommended for autonomous programs.

```java
// set the run mode
m_motor.setRunMode(Motor.RunMode.VelocityControl);

// set and get the coefficients
m_motor.setVeloCoefficients(0.05, 0.01, 0.31);
double[] coeffs = m_motor.getVeloCoefficients();
double kP = coeffs[0];
double kI = coeffs[1];
double kD = coeffs[2];

// set and get the feedforward coefficients
m_motor.setFeedforwardCoefficients(0.92, 0.47);
double[] ffCoeffs = m_motor.getFeedforwardCoefficients();
double kS = ffCoeffs[0];
double kV = ffCoeffs[1];

m_motor.setFeedforwardCoefficients(0.92, 0.47, 0.3);
ffCoeffs = m_motor.getFeedforwardCoefficients();
kA = ffCoeffs[2];

// set the velocity of the motor
// this must be called in a control loop
m_motor.set(-0.54); // 54% of maximum velocity, backwords
```

### Setting Behaviors

SolversLib, like the SDK, has wrapper methods for setting the ZeroPowerBehavior and the direction of the motors. ZeroPowerBehavior is often used for mechanisms other than the drivetrain; for example, like how you would use the BRAKE behavior for a lift.

```java
// set the inversion factor
m_motor.setInverted(true);

// get the inversion factor
boolean isInverted = m_motor.getInverted();

// set the zero power behavior to BRAKE
m_motor.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);

```

### The Built-In Encoder

A lot of motors have built-in encoders. SolversLib offers a nested class `Motor.Encoder` that utilizes advanced mechanics and corrects for [velocity overflow](https://github.com/FIRST-Tech-Challenge/SkyStone/issues/241). One of the other great things is that resetting the encoder does not require stopping the motor. It calculates an offset and subtracts that from the current position. This offset is set to the current position of the encoder each time the `reset()` method is called. The Motor object also has methods that manipulate the encoder so that you don't have to grab the internal encoder instance from the object.

You can also use the built-in encoder as an encoder itself when using an external encoder.

```java
// reset the encoder
m_motor.resetEncoder();

// the current position of the motor
int pos = m_motor.getCurrentPosition();

// get the current velocity
double velocity = m_motor.getVelocity(); // only for MotorEx
double corrected = m_motor.getCorrectedVelocity();

// grab the encoder instance
Motor.Encoder encoder = m_motor.encoder;

// get number of revolutions
double revolutions = encoder.getRevolutions();

// set the distance per pulse to 18 mm / tick
encoder.setDistancePerPulse(18.0);
m_motor.setDistancePerPulse(18.0); // also an option

// get the distance traveled
double distance = encoder.getDistance();
distance = m_motor.getDistance(); // also an option

/** USEFUL FEATURES **/

// you can set the encoder of the motor to a different motor's
// encoder
m_motor.encoder = other_motor.encoder;

// sometimes the encoder needs to be reset completely
// through hardware
m_motor.stopAndResetEncoder();
```

## The MotorEx Object

`MotorEx` is an implementation of the Motor class with better integrated velocity control. Unlike the Motor object, it uses the corrected velocity by default instead of the raw velocity returned by the SDK's encoder estimates. It also uses the `DcMotorEx` object instead of the `DcMotor`. Calling `getVelocity()` will return the velocity.

You can also set the velocity directly using `setVelocity()`. You can pass the angular rate and the angle unit (optional). Passing just the angular rate will set the velocity in ticks per second. Passing an angle unit will set the velocity to units per second, depending on the unit that is passed into the method.

Additionally, `MotorEx` supports power caching. If the power set to that hardware is less than an adjustable threshold, it will not send a write to it to help with loop speeds. The default threshold, which is called `cachingTolerance` , is 0.0001.&#x20;

You can use `.setCachingTolerance` to adjust `cachingTolerance` it as needed.

```java
MotorEx m_motor = new MotorEx(hardwareMap, "motorEx");

m_motor.setCachingTolerance(0.0001);
```

{% hint style="warning" %}
This default value of `0.0001`  effectively disables the feature. To actually see results with power caching, increase it to `0.01` or higher depending on the precision required.
{% endhint %}

### Bulk Reading

A bulk read reads all of the sensor data (except I2C) on a lynx module to save cycle times. Bulk reads were introduced in SDK version 5.4. Since SolversLib uses wrappers, we can treat them the same way as other sensors.

Here's a sample implementation of auto-caching.

```java
// obtain a list of hubs
List<LynxModule> hubs = hardwareMap.getAll(LynxModule.class);

MotorEx m1 = new MotorEx(hardwareMap, "one");
MotorEx m2 = new MotorEx(hardwareMap, "two");

for (LynxModule hub : hubs) {
     hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}

// control loop
int cycles = 0;
while (cycles++ < 500) {
    double v1 = m1.getVelocity();
    double v2 = m2.getVelocity();

    /* implementation */
}
```

You can also take a look at [this sample](https://github.com/FIRST-Tech-Challenge/FtcRobotController/blob/master/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java) in the SDK.

## MotorGroup

A motor group object takes several motors and runs them in parallel like a single motor. Motor groups have one leader and a set of followers. For any group, there *must* be a leader, but the number of followers can be zero. This makes creating different drive profiles simpler. The constructor for a `MotorGroup` is as follows:

```java
MotorGroup myMotors = new MotorGroup(leader, follower1, follower2, ...);
```

The number of followers is variable. The other methods of the `MotorGroup` are the same as the ones found in `Motor`. You can very simply treat a `MotorGroup` object like a single `Motor` object. The [flywheel sample](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/FlywheelSample.java) in the examples folder shows a few other methods you can utilize with the `MotorGroup`.

{% hint style="danger" %}
**Warning:** CRServo/CRServoEx should use a CRServoGroup instead of a [MotorGroup](#motorgroup)
{% endhint %}


# Sensors

package com.seattlesolvers.solverslib.hardware

## Sensors

There are a few sensors that are offered in SolversLib:

* [SensorColor](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorColor.java) & [SensorRevColorV3](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorRevColorV3.java)
* [SensorDistance](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistance.java) & [SensorDistanceEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorDistanceEx.java)
* [SensorRevTOFDistance](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SensorRevTOFDistance.java)

The `SensorColor` class is just an extension for the `ColorSensor` class that is in the SDK, and the `SensorRevColorV3` is the same but for the [REV Color Sensor V3](https://www.revrobotics.com/rev-31-1557/).

`SensorDistance` and `SensorDistanceEx` are interfaces for creating custom distance sensors if desired. An implementation of the `SensorDistanceEx` interface is `SensorRevTOFDistance` which utilizes the time-of-flight mechanic to track distance. It has a minimum and maximum tolerance, that can be used via `DistanceTarget`.

## Gyro Extensions

The `GyroEx` class is an extended gyro that allows users to add more configurable methods and possible control to their gyro. An example would be creating a `ModernRoboticsGyro` class. The abstract class has the following methods:

* `init()`: initializes the gyro and sets the current direction to the 0 heading
* `getHeading()`: returns the heading of the robot compared to the last reset
* `getAbsoluteHeading()`: returns the absolute heading relative to the initial direction
* `getAngles()`: returns the x, y, and z orientation of the gyro. This is functionally the same as yaw, pitch, and roll.
* `getRotation2d()`: transforms the heading into a `Rotation2d` object
* `reset()`: applies an offset so that `getHeading()` returns the 0 position

A useful implementation of this is the [RevIMU](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/RevIMU.java) class for the built-in imu on your REV hub.


# Deprecated Hardware

Deprecated Classes & Interfaces

### SolversHardware

SolversLib used to offers much simpler motor wrappers for motor caching & easy usage of [Axon Servos](https://axon-robotics.com/collections/servos). However, it is now deprecated, as these features have been merged into SolversLib's [native hardware classes](/0.3.4/features/hardware), which has been rewritten to better support it.

### ServoEx Interface and SimpleServo Class

Additionally, the [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) interface and [SimpleServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SimpleServo.java) class have been Deprecated, in favor of the new and superior [ServoEx](https://github.com/FTC-23511/SolversLib/blob/public-beta/core/src/main/java/com/seattlesolvers/solverslib/hardware/servos/ServoEx.java) class (which has [updated documentation](/0.3.4/features/hardware/servos#servoex)). A copy of the original ServoEx interface documentation is below.

The [ServoEx](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/ServoEx.java) interface allows for more methods and actions than the normal servo class in the SDK. You can change the position of the servo relative to the last position or set it to an absolute position. You can either specify a position within the range of the servo's motion or have it rotate a certain number of specified angle units.

An example implementation of this can be found in the [SimpleServo](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/hardware/SimpleServo.java) class. You can create a simple servo like this:

```java
ServoEx servo = new SimpleServo(
    hardwareMap, "servo_name", MIN_ANGLE, MAX_ANGLE
);

// the above is functionally equivalent to
servo = new SimpleServo(
    hardwareMap, "servo_name", MIN_ANGLE, MAX_ANGLE,
    AngleUnit.DEGREES
);

// if you want to set the range in radians in the constructor
// you can use the following
servo = new SimpleServo(
    hardwareMap, "servo_name", MIN_ANGLE, MAX_ANGLE,
    AngleUnit.RADIANS
);
```

`MIN_ANGLE` and `MAX_ANGLE` are the minimum and maximum angle positions in degrees you would like to set the servo. This functionally serves as the servo's effective range. If you want to change the effective range at any point, you can do the following:

```java
// change the effective range to a min and max in DEGREES
servo.setRange(MIN_ANGLE, MAX_ANGLE);

// change the range to a min and max in RADIANS
servo.setRange(MIN_ANGLE, MAX_ANGLE, AngleUnit.RADIANS);

// return the effective range
double degreeRange = servo.getAngleRange();

// return the effective range in RADIANS
degreeRange = servo.getAngleRange(AngleUnit.RADIANS);
```

You can invert the servo's direction as well:

```java
// invert the servo
servo.setInverted(true);

// get if the servo is inverted (true if inverted, false if not)
boolean isInverted = servo.getInverted();
```

To turn to positions and angles, utilize the following methods:

* `rotateByAngle`: turns the servo a number of angle units relative to the current angle
* `turnToAngle`: sets the absolute angle of the servo
* `rotateBy`: turns the servo a relative positional distance from the current position
* `setPosition`: set the absolute position of the servo (from 0 to 1)


# Gamepad

package com.seattlesolvers.solverslib.gamepad

SolversLib provides enhanced Gamepad features. These classes are essentially extensions of the stock FTC SDK Gamepad features but with easier implementation methods.

## GamepadKeys

Provides enum representations of the buttons, D-Pad, bumpers, and triggers. Buttons, D-Pad, and bumpers are stored in `GamepadKeys.Button` and triggers are stored in `GamepadKeys.Trigger`. SolversLib has updated support for PS4 / PS5 controllers as well.

| Buttons              |
| -------------------- |
| Y                    |
| X                    |
| A                    |
| B                    |
| TRIANGLE             |
| CIRCLE               |
| SQUARE               |
| CROSS                |
| LEFT\_BUMPER         |
| RIGHT\_BUMPER        |
| BACK                 |
| START                |
| DPAD\_UP             |
| DPAD\_DOWN           |
| DPAD\_LEFT           |
| DPAD\_RIGHT          |
| LEFT\_STICK\_BUTTON  |
| RIGHT\_STICK\_BUTTON |
| PS                   |
| SHARE                |
| TOUCHPAD             |
| TOUCHPAD\_FINGER\_1  |
| TOUCHPAD\_FINGER\_1  |

| Trigger        |
| -------------- |
| LEFT\_TRIGGER  |
| RIGHT\_TRIGGER |

```java
// these are from the GamepadButton class that is used
// for command-based frameworks
GamepadButton grabButton = new GamepadButton(
    gamepad1, GamepadKeys.Button.A
);
GamepadButton releaseButton = new GamepadButton(
    gamepad2, GamepadKeys.Button.B
);

GamepadEx gamepadEx = new GamepadEx(gamepad1);
```

## GamepadEx

An extension of the stock FTC SDK `Gamepad` class. Constructed simply from a Gamepad. Provides six intuitive value-getting methods:

* `getButton()`: Given a `GamepadKeys.Button`, this method will check if that Button is pressed, returning a boolean of whether that Button is pressed.

```java
gamepadEx.getButton(GamepadKeys.Button.A);
```

* `getTrigger()`: Given a `GamepadKeys.Trigger`, this method will return the value of the Trigger (0 if unpressed, 1 if fully depressed).

```java
gamepadEx.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
```

* `getLeftY()`: Returns the value of the y-axis of the left joystick (note that the value returned is the opposite of what would be returned from the standard gamepad object).

```java
gamepadEx.getLeftY();
```

* `getRightY()`: Returns the value of the y-axis of the right joystick

```java
gamepadEx.getRightY();
```

* `getLeftX()`: Returns the value of the x-axis of the left joystick

```java
gamepadEx.getLeftX();
```

* `getRightX()`: Returns the value of the x-axis of the right joystick

```java
gamepadEx.getRightX();
```

## KeyReader

The `KeyReader` interface is the base for objects that monitor an individual button or trigger on a gamepad. All `Reader` classes must implement these functions:

* `readValue()`: Reads the current value of the key, true or false, and updates the values used by the reader. Returns nothing. This must be called once every loop.
* `isDown()` : Checks if key is currently down. Will return a boolean of whether that key is pressed.
* `wasJustPressed()` : Returns boolean whether the key is pressed, but only if it was previously not pressed.
* `wasJustReleased()` : Returns boolean indicating whether the key is not pressed, but only if it was previously pressed.
* `stateJustChanged` : Returns boolean indicating that the key's value has switched.

## TriggerReader

The `TriggerReader` class implements the `KeyReader` interface. Because `GamepadEx` Triggers return a `double` , the `TriggerReader` class interprets a value of greater than `0.5` as a trigger press.

The following constructs a new Trigger Reader with a `GamepadEx` gamepad and `GamepadKeys.Trigger` trigger.

```java
TriggerReader triggerReader = new TriggerReader(
    gamepadEx, GamepadKeys.Trigger.RIGHT_TRIGGER
);
```

Below are the different methods you can use with the trigger reader.

```java
triggerReader.isDown();
triggerReader.readValue();
triggerReader.stateJustChanged();
triggerReader.wasJustPressed();
triggerReader.wasJustReleased();
```

## ButtonReader

The `ButtonReader`class implements the `KeyReader` interface. It checks if a button is pressed, released, or is down.

```java
ButtonReader reader = new ButtonReader(
    gamepadEx, GamepadKeys.Button.A
);
```

* `ButtonReader(GamepadEx gamepad, GamepadKeys.Button button)`: Constructs a new Button Reader with a `GamepadEx` gamepad and a `GamepadKeys.Button` button.
* `ButtonReader(BooleanSupplier supplier)`: Constructs a new Button Reader using the value of a boolean supplier instead of a gamepad, which allows reading value states easily without a gamepad.

```java
reader.readValue();
reader.wasJustPressed();
reader.stateJustChanged();
reader.isDown();
reader.wasJustReleased();
```

The `GamepadEx` objects actually contain `ButtonReader`s. For every `GamepadKeys.Button`, there is a matching `ButtonReader` entry in the map. It is stored internally as a `Map<GamepadKeys.Button, ButtonReader>`. This allows you to use these features just with the `GamepadEx` class.

```java
// create the gamepad
GamepadEx myGamepad = new Gamepad(gamepad1);

/** The methods for using the ButtonReaders **/
myGamepad.wasJustPressed(GamepadKeys.Button.A);
myGamepad.stateJustChanged(GamepadKeys.Button.A);
myGamepad.isDown(GamepadKeys.Button.A);
myGamepad.wasJustReleased(GamepadKeys.Button.A);

// pass the GamepadKeys.Button that you want to read
// into the method argument

// to read all buttons at once, perform a single call
myGamepad.readButtons();
/*
this is the equivalent of calling readValue() once
for all your readers
*/
```

## ToggleButtonReader

```java
ToggleButtonReader toggleButtonReader = new ToggleButtonReader(
    gamepadEx, GamepadKeys.Button.A
);
```

The `ToggleButtonReader` class extends `ButtonReader` and adds the ability to get the status of a toggle. `readValue()` needs to be run in a loop to get the state of the toggle.

`getState()` : Gets the toggle value of a button or boolean supplier.

```java
toggleButtonReader.getState();
```

### Usage

```java
GamepadEx toolOp = new GamepadEx(gamepad2);
ToggleButtonReader aReader = new ToggleButtonReader(
  toolOp, GamepadKeys.Button.A
);

while (...) {
  if (aReader.getState()) {
    // if toggle state true
  } else {
    // if toggle state false
  }
  aReader.readValue();
}
```


# Drivebases

package com.seattlesolvers.solverslib.drivebase

All of the [SolversLib drivebase classes](https://github.com/FTC-23511/SolversLib/tree/dev/core/src/main/java/com/arcrobotics/ftclib/drivebase) are based on the `RobotBase` abstract class. This is functionally similar to the [RobotDriveBase ](https://github.com/wpilibsuite/allwpilib/blob/50db653f8d864c594c6a9ab7dd5a0f45b4483a03/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java)class in WPILib. It contains:

* An enumerator for the different motor types (i.e. the indices of the motors in the array)
* Maximum possible speed for the drivebase to approach
* A clipping method between a minimum and maximum value
* Normalization of wheel speeds
* A `stop()` method
* Square input method

All of the following drive types are available for use in SolversLib. All that's needed for the user to do is plug in the values to an instantiation of the object.

## Differential

A differential drive is one that has two motors or motor groups on either side of the robot. Each side acts as a connected set or motor group. There are two types of drive systems here. You can use the tank and arcade control schemes with a differential drive.

![CAD by Hrithik and Sanjay from FTC Team 16439](/files/eVAaw8fecrYScEWOTmJ8)

Creating the drivetrain object is simple:

```java
DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
```

The constructor has two parameters: left and right. For a differential drive, it considers each side as a single motor. As a result, you need to pass in a motor group object.

### Arcade

Arcade drive use a y-value input from the controller and a value from the turn stick. We know that when the turn stick is pushed left, the right side should move forward and the left side should move backwards. Therefore, since pushing the turn stick to the left returns a negative value, it should be added to the left speed and subtracted from the right speed.

Here is how to call the method for arcade drive. An additional boolean parameter is optional, which is `squareInputs`. By default, this value is false.

```java
m_drive.arcadeDrive(forwardSpeed, turnSpeed);
```

### Tank

Tank drive uses a y-value input from the left and right sticks. The sticks control their respective side of the robot.

```java
m_drive.tankDrive(leftSpeed, rightSpeed);
```

## Holonomic

In SolversLib, we shortened holonomic drive to the generic term H-Drive. A holonomic drive is one that can move in omnidirectional space, which is why it is often called an omnidirectional drive. There are different sub-types of holonomic drivebases.

### Holonomic Drivebases

#### Three Wheel Holonomic

A three wheel holonomic drivebase, otherwise known as a "Kiwi" or "Killough," is a drivetrain with omnidirectional motion while utilizing three omniwheels.

![Rendered by Pranay from FTC 16236, CAD made by Eric from FTC 18246](/files/CWnJ9cGWHPXHykaE319Q)

You can create the kiwi drive as such:

```java
// for if the wheels are oriented at 60 degrees,
// 120 degrees, and 270 degrees
HDrive kiwi_A = new HDrive(left, right, slide);

// for if the wheels are oriented at other angles
HDrive kiwi_B = new HDrive(left, right, slide,
                           leftAngle, rightAngle, slideAngle);
```

Your slide motor is generally the back of the kiwi drive and the others are self-explanatory. Note that the motor angles above are in Radians. Ensure that you're associating the correct motor/wheel with the corresponding argument as they *don't* go around the robot in order. Forward is 0 degrees.

#### X-Drive

An X-drive is a holonomic base that has four omniwheels positioned into an "X" shape as seen below.

![An X-Drive concept from VEX](/files/pJvYcI50RIwyet7yQa0V)

You can create the x-drive drive as such:

```java
// input motors exactly as shown below
HDrive xDrive = new HDrive(frontLeft, frontRight,
                           backLeft, backRight);
```

#### Mecanum

A mecanum drivebase is a type of holonomic drive that utilizes [mecanum wheels](https://en.wikipedia.org/wiki/Mecanum_wheel) for movement.

![A custom parallel plate mecanum drivetrain](/files/WbgOQxWPTP47yG4S9USO)

For more information on mecanum drives, please watch this video:

{% embed url="<https://www.youtube.com/watch?v=8rhAkjViHEQ>" %}

You can create the mecanum drive as such:

```java
// input motors exactly as shown below
MecanumDrive mecanum = new MecanumDrive(frontLeft, frontRight,
                                        backLeft, backRight);
```

### Control Scheme

Like how the differential drive has two different control schemes, so too does holonomic. There is field-centric and robot-centric. The only difference is that there is some sort of interpolation of the robot's heading (the direction the forward face is pointing globally).

#### Robot-Centric

Robot-centric assumes that each push of the joystick is in relation to the local position of the robot—this means that whenever the user pushes the drive stick forward, the robot will drive in the direction of its front-facing side.

For all types of holonomic drive you do this by calling the .`driveRobotCentric()` method that takes the gamepad inputs and converts them into directional values.

```java
m_drive.driveRobotCentric(strafeSpeed, forwardSpeed, turnSpeed)
```

#### Field-Centric

Field-centric assumes that each push of the joystick is in relation to the global position of the robot—this means that whenever the user pushes the drive stick forward, the robot will move away from the driver no matter its orientation. This is done by rotating the direction of the joystick clockwise by an angle measurement equivalent to the global heading of the robot.

For all types of holonomic drive you do this by calling the .`driveFieldCentric()` method that takes the gamepad inputs and converts them into directional values. Additionally, you include the reading for the heading in degrees.

```java
m_drive.driveFieldCentric(strafeSpeed, forwardSpeed, turn, heading);
```

The `heading` argument is the current heading of the robot, usually from the IMU. Note that it is in *degress* here, not radians.

### Sample

{% code title="BasicDriveOpMode.java" %}

```java
@TeleOp
public class BasicDriveOpMode extends OpMode {

    private Motor fL, fR, bL, bR;
    private MecanumDrive drive;
    private GamepadEx driverOp;

    @Override
    public void init() {
        /* instantiate motors */
        
        drive = new MecanumDrive(fL, fR, bL, bR);
        driverOp = new GamepadEx(gamepad1);
    }
    
    @Override
    public void loop() {
        drive.driveRobotCentric(
            driverOp.getLeftX(),
            driverOp.getLeftY(),
            driverOp.getRightY()
        );
    }

}
```

{% endcode %}

You can find an additional sample [here](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrivingSample.java) which is more thoroughly detailed.


# Geometry

package com.seattlesolvers.solverslib.geometry

SolversLib provides access to geometry classes taken from WPILib. Since we like copy-pasting straight from WPILib instead of linking to the [original material](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/geometry/pose.html), that's what we're gonna do.

## Translation

Translation in 2 dimensions is represented by SolversLib's`Translation2d` class. This class has an x and y component, representing the point $$(x,y)$$ or the vector $$\begin{bmatrix} x\ y \end{bmatrix}$$ on a 2-dimensional coordinate system.

You can get the distance to another `Translation2d` object by using the `getDistance(Translation2d other)`, which returns the distance to another `Translation2d` by using the Pythagorean theorem.

## Rotation

Rotation in 2 dimensions is represented by SolversLib’s `Rotation2d` class. This class has an angle component, which represents the robot’s rotation relative to an axis on a 2-dimensional coordinate system. Positive rotations are counterclockwise.

## Pose

Pose is a combination of both translation and rotation and is represented by the `Pose2d` class. It can be used to describe the pose of your robot in the field coordinate system, or the pose of objects, such as vision targets, relative to your robot in the robot coordinate system. `Pose2d` can also represent the vector $$\begin{bmatrix} x\ y\ \theta \end{bmatrix}$$ .

## Vector

A vector in 2 dimensions is represented by the `Vector2d` class. It holds an $$x$$ and a $$y$$ value similarly to a `Translation2d`. These components representing the point $$(x,y)$$ or as the matrix$$\begin{bmatrix} x\ y \end{bmatrix}$$.

Unlike a `Translation2d`, there are a few different methods and features.

## Transform and Twist

SolversLib provides 2 classes, `Transform2d`, which represents a transformation to a pose, and `Twist2d` which represents a movement along an arc. `Transform2d` and `Twist2d` all have $$x$$ , $$y$$ and $$\theta$$ components.

`Transform2d` represents a **relative** transformation. It has an translation and a rotation component. Transforming a `Pose2d` by a `Transform2d` rotates the translation component of the transform by the rotation of the pose, and then adds the rotated translation component and the rotation component to the pose. In other words, `Pose2d.plus(Transform2d)` returns $$\begin{bmatrix} x\_{p} \ y\_{p} \ \theta\_{p} \end{bmatrix} + \begin{bmatrix} \cos{\theta\_{p}} & -\sin{\theta\_{p}} & 0 \ \sin{\theta\_{p}} & \cos{\theta\_{p}} & 0 \ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\_{t} \ y\_{t} \ \theta\_{t} \end{bmatrix}$$ .

`Twist2d` represents a change in distance along an arc. For a given arc traveled, $$x$$ is the distance traveled forward as measured from the robot's perspective throughout the movement (for a differential drive, this is the arc length), $$y$$ is the distance traveled sideways from the robot's perspective (for a differential drive, this is 0), and $$\theta$$ is the change in heading.

Both classes can be used to estimate robot location. `Twist2d` is used in some of the SolversLib odometry classes to update the robot’s pose based on movement, while `Transform2d` can be used to estimate the robot’s global position from vision data.


# Controllers

package com.seattlesolvers.solverslib.controller

In SolversLib, there are controllers that can improve the motion of mechanisms in FTC. This includes PID, feedforward, and SquID control.&#x20;

{% hint style="success" %}
If you wish to use any one of the controllers mentioned above directly, you can look at the individual subpages for them directly
{% endhint %}

The primary controller is an abstract class called `Controller`, which is a generic closed-loop controller. You can find the source code for the `Controller` class [here](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/controller/Controller.java), and the theory behind it on CtrlAltFtc [here](https://www.ctrlaltftc.com/introduction-to-closed-loop-control).

{% hint style="info" %}
All other controllers (PID/PIDF, FF, SquIDF) extend this controller in some manner, and inherits its protected fields and methods. They provide added functionality that will be usually be more helpful to most users.
{% endhint %}

The `Controller` class serves as the foundation for all control algorithms in SolversLib. It provides common functionality for setpoint tracking, error calculation, tolerance checking, and output management. Concrete implementations (like PID, Feedforward, or SquID) extend this class and implement the `calculateOutput()` method with their specific control logic.

There are 3 key points that are a bit more specific for SolversLib's `Controller`:&#x20;

* **Minimum Output**: When not at setpoint, the absolute value of the output will be at least `minOutput` (if it is set), preserving the sign of the correction (using `Math.signum()`)
* **Error Calculation**: Error is automatically updated whenever `setSetPoint()` is called
* **atSetpoint()**: Returns `true` only when **both position and velocity errors** are within tolerance - this is especially important for PID/PIDF classes

This class is generally used for advanced purposes, like SolversLib's[ Point-to-Point](/0.3.4/pathing/point-to-point) drivetrain control system.

### Using the Controller Class

in order to use SolversLib's Controller, users must first construct a `Controller` object with its only constructor:

```java
Controller controller = new Controller();
```

This constructor is implicitly called during construction of the other controllers.

#### Methods

1. **`double calculate(double pv)`**

Calculates the control output using the given measured value (process variable).

**Parameters:**

* `pv` - The current measured value

**Returns:** The control value u(t), respecting minimum output if not at setpoint

2. **`double calculate(double pv, double sp)`**

Calculates the control output with a new setpoint and measured value.

**Parameters:**

* `pv` - The current measured value
* `sp` - The desired setpoint

**Returns:** The control value u(t)

3. **`double calculate()`**

Calculates the control output using the stored measured value.

**Returns:** The control value u(t)

**Note:** Requires `measuredValue` to be set beforehand

4. **`void setSetPoint(double sp)`**

Sets the controller's target setpoint and updates error calculations.

**Parameters:**

* `sp` - The desired setpoint

**Side Effects:**

* Updates `errorVal_p` (positional error)
* Updates `errorVal_v` (velocity error)

5. **`double getSetPoint()`**

Returns the current setpoint.

**Returns:** The current target setpoint

6. **`boolean atSetPoint()`**

Checks if the controller has reached the setpoint within the defined tolerances.

**Returns:** `true` if both positional and velocity errors are within tolerance, `false` otherwise

7. **`double getPositionError()`**

Returns the current positional error e(t).

**Returns:** The difference between setpoint and measured value

8. **`double getVelocityError()`**

Returns the current velocity error e'(t).

**Returns:** The rate of change of the error

9. **`void setTolerance(double positionTolerance)`**

Sets the acceptable positional error for `atSetPoint()` checks. Velocity tolerance remains at infinity.

**Parameters:**

* `positionTolerance` - Maximum acceptable positional error

**Example:**

```java
controller.setTolerance(2.0); // Within 2 units is acceptable
```

10. **`void setTolerance(double positionTolerance, double velocityTolerance)`**

Sets both positional and velocity error tolerances.

**Parameters:**

* `positionTolerance` - Maximum acceptable positional error
* `velocityTolerance` - Maximum acceptable velocity error
*

11. **`double[] getTolerance()`**

Returns the current tolerance settings.

**Returns:** Array containing `[positionTolerance, velocityTolerance]`

12. **`void reset()`**

Resets the controller state by clearing previous error and timestamp values.

**Use Case:** Call this when switching between different control tasks or when re-enabling a disabled controller

13. **`Controller setMinimumOutput(double minOutput)`**

Enforces a minimum output magnitude when the controller is not at the setpoint. This prevents the controller from applying insignificantly small corrections.

**Parameters:**

* `minOutput` - Minimum absolute value of the output (sign is preserved)

**Returns:** The controller instance (for method chaining)

14. **`double getPeriod()`**

Returns the time period between controller calculations.

**Returns:** The period in seconds (or your time unit)

### Protected Methods

`abstract double calculateOutput(double pv)`

This is the core control algorithm that is implemented by subclasses.

**Parameters:**

* `pv` - The measured value

**Returns:** The raw control output before minimum output enforcement

**Note:** This method is called internally by `calculate()`. Subclasses override this to implement specific control strategies (PID, feedforward, etc.).


# PID and PIDF

package com.seattlesolvers.solverslib.controller

## PID Control

The following post from Noah in the [FTC Discord](https://discord.gg/first-tech-challenge) best explains PID control.

> You'll hear the term PID Controller used a lot (F is tacked on sometimes) in robotics. It's relatively simple and pretty effective at a lot of simple tasks. A PID controller is a form of "closed loop control." This basically just means you're altering an input to some "plant" based on feedback. This concept applies to a wide range of actions but we'll take a look at velocity PID control as that is what's relevant for this year's game. So say you have a goBILDA 3:1 1620RPM motor powering a flywheel. You want that flywheel to spin at a constant speed to ensure consistency between your shots. So you run a `motor.setPower(0.5)` which sends 50% of 12v to the motor. The motor is getting a 6v signal (technically not true bc of PWM but that's another topic). The motor should be running at 810 RPM right? That's 50% of 1620RPM. Chances are, it's not actually running at this speed. Motors have +- 10% tolerance between them. The voltage-torque curve isn't linear. Or there is something resisting the motor (like the inertia of the flywheel) so it takes extra power to get it up to that speed. So how do we actually ensure that our motor is running at exactly 810RPM? Most FTC motors come with an encoder built it. This allows us to measure the velocity of the output shaft. So with the encoder all hooked up, we know that our motor isn't spinning as fast as we want it to. But how do we actually correct for this? You slap a PID Controller on it. A PID controller is a pretty basic controller (despite the daunting equation when you look it up) that basically responds to your the difference between your measured velocity and desired velocity (error) and will add more or less power based on error. You just check the velocity every loop, feed the value in the controlled, and it gives you the power you want to set it to the desired velocity.

For information on the theory behind PID control, we recommend reading [this page](https://gm0.org/en/stable/docs/software/control-loops.html) in gm0 and [this page](https://www.ctrlaltftc.com/the-pid-controller) in CtrlAltFtc.&#x20;

The following video does a good job explaining each gain:

{% embed url="<https://www.youtube.com/watch?v=XfAt6hNV8XM>" %}

Our base class is `PIDFController` for the SolversLib PID control scheme. This class performs the calculations for PIDF, which are proportional, integral, derivative, and feedforward values. The additional F term is an additional gain for creating offset, for purposes like maintaining a position, counteracting weight/gravity, or overcoming friction.

### Using the PIDFController Class

#### Constructing a PIDFController

In order to use SolversLib's PIDF control functionality, users must first construct a `PIDFController` object with the desired gains:

```java
// Creates a PIDFController with gains kP, kI, kD, and kF
PIDFController pidf = new PIDFController(kP, kI, kD, kF);

/*
 * Here are the constructors for the other controllers
 */
PIDController pid = new PIDController(kP, kI, kD);
PDController pd = new PDController(kP, kD);
PController p = new PController(kP);
```

You can also pass in an additional two parameters: the setpoint and previous value. The default values for these are 0.

You can also change the gain constants even after creating the controller object.

```java
// set our gains to some value
pidf.setP(0.37);
pidf.setI(0.05);
pidf.setD(1.02);

// get our gain constants
kP = pidf.getP();
kI = pidf.getI();
kD = pidf.getD();

// set all gains
pidf.setPIDF(kP, KI, kD, 0.7);

// get all gain coefficients
double[] coeffs = pidf.getCoefficients();
kP = coeffs[0];
kI = coeffs[1];
kD = coeffs[2];
kF = coeffs[3];
```

These gains must be tuned. An example of tuning PID can be found on [this page](https://www.learnroadrunner.com/drive-velocity-pid-tuning.html) of Learn Road Runner.

#### Using the Feedback Loop Output

The `calculate()` method should be called each iteration of the control loop. The controller uses timestamps to calculate the difference in time between each call of the method, which means it adjusts based on the loop time. You can obtain the cycle time of your current loop iteration by calling `getPeriod()`.

Using the constructed `PIDFController` is simple: call the `calculate()` method from the main loop.

```java
// Calculates the output of the PIDF algorithm based on sensor
// readings. Requires both the measured value
// and the desired setpoint
double output = pidf.calculate(
    motor.getCurrentPosition(), setpoint
);

/*
 * A sample control loop for a motor
 */
PController pController = new PController(kP);

// We set the setpoint here.
// Now we don't have to declare the setpoint
// in our calculate() method arguments.
pController.setSetPoint(1200);

// perform the control loop
/*
 * The loop checks to see if the controller has reached
 * the desired setpoint within a specified tolerance
 * range
 */
while (!pController.atSetPoint()) {
  double output = pController.calculate(
    m_motor.getCurrentPosition()  // the measured value
  );
  m_motor.setVelocity(output);
}
m_motor.stopMotor(); // stop the motor

// NOTE: motors have internal PID control
```

#### Checking Errors

The methods `getPositionError()` and `getVelocityError()` are named assuming that the loop is controlling a position - for a loop that is controlling a velocity, these return the velocity error and the acceleration error, respectively.

The current error of the measured process variable is returned by the `getPositionError()` function, while its derivative is returned by the `getVelocityError()` function.

#### Specifying and Checking Tolerances

If only a position tolerance is specified, the velocity tolerance defaults to infinity.

As above, “position” refers to the process variable measurement, and “velocity” to its derivative - thus, for a velocity loop, these are actually velocity and acceleration, respectively.

Occasionally, it is useful to know if a controller has tracked the setpoint to within a given tolerance - for example, to determine if a command should be ended, or (while following a motion profile) if motion is being impeded and needs to be re-planned.

To do this, we first must specify the tolerances with the `setTolerance()` method; then, we can check it with the `atSetPoint()` method.

```java
// Sets the error tolerance to 5, and the error derivative
// tolerance to 10 per second
pidf.setTolerance(5, 10);

// Returns true if the error is less than 5 units, and the
// error derivative is less than 10 units
pidf.atSetPoint();
```

#### Resetting the Controller

It is sometimes desirable to clear the internal state (most importantly, the integral accumulator) of a `PIDFController`, as it may be no longer valid (e.g. when the `PIDFController` has been disabled and then re-enabled). This can be accomplished by calling the `reset()` method.


# Feedforward Helpers

package com.seattlesolvers.solverslib.controller

## Feedforward Control

So far, we’ve used feedback control for reference tracking (making a system’s output follow a desired reference signal). While this is effective, it’s a reactionary measure; the system won’t start applying control effort until the system is already behind. If we could tell the controller about the desired movement and required input beforehand, the system could react quicker and the feedback controller could do less work. A controller that feeds information forward into the plant like this is called a feedforward controller.

A feedforward controller injects information about the system’s dynamics (like a mathematical model does) or the intended movement. Feedforward handles parts of the control actions we already know must be applied to make a system track a reference, then feedback compensates for what we do not or cannot know about the system’s behavior at runtime.

There are two types of feedforwards: model-based feedforward and feedforward for unmodeled dynamics. The first solves a mathematical model of the system for the inputs required to meet desired velocities and accelerations. The second compensates for unmodeled forces or behaviors directly so the feedback controller doesn’t have to. Both types can facilitate simpler feedback controllers. We’ll cover several examples below.

SolversLib provides a number of classes to help users implement accurate feedforward control for their mechanisms. In many ways, an accurate feedforward is more important than feedback to effective control of a mechanism. Since most FTC mechanisms closely obey well-understood system equations, starting with an accurate feedforward is both easy and hugely beneficial to accurate and robust mechanism control.

SolversLib currently provides the following three helper classes for feedforward control. The feedforward components will calculate outputs in units determined by the units of the user-provided feedforward gains. Users *must* take care to keep units consistent as it does not have a type-safe unit system.

### SimpleMotorFeedforward

The `SimpleMotorFeedforward` class calculates feedforwards for mechanisms that consist of permanent-magnet DC motors with no external loading other than friction and inertia, such as flywheels and robot drives.

To create a `SimpleMotorFeedforward`, simply construct it with the required gains:

```java
// Create a new SimpleMotorFeedforward with gains kS, kV, and kA
SimpleMotorFeedforward feedforward =
    new SimpleMotorFeedforward(kS, kV, kA);
```

Please note that the `kA` value is optional. If the mechanism does not have much inertia, then it is not required.

To calculate the feedforward, simply call the `calculate()` method with the desired motor velocity and acceleration:

```java
// Calculates the feedforward for a velocity of 10 units/second
// and an acceleration of 20 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20);
```

### ArmFeedforward

The `ArmFeedforward` class calculates feedforwards for arms that are controlled directly by a permanent-magnet DC motor, with external loading of friction, inertia, and mass of the arm. This is an accurate model of most arms in FTC.

To create an `ArmFeedforward`, simply construct it with the required gains:

```java
// Create a new ArmFeedforward with gains kS, kCos, kV, and kA
ArmFeedforward feedforward = new ArmFeedforward(kS, kCos, kV, kA);
```

To calculate the feedforward, simply call the `calculate()` method with the desired arm position, velocity, and acceleration:

```java
// Calculates the feedforward for a position of 1 units,
// a velocity of 2 units/second, and
// an acceleration of 3 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(1, 2, 3);
```

### ElevatorFeedforward

The `ElevatorFeedforward` class calculates feedforwards for elevators that consist of permanent-magnet DC motors loaded by friction, inertia, and the mass of the elevator. This is an accurate model of most elevators in FTC.

To create a `ElevatorFeedforward`, simply construct it with the required gains:

```java
// Create a new ElevatorFeedforward with gains kS, kG, kV, and kA
ElevatorFeedforward feedforward = new ElevatorFeedforward(
    kS, kG, kV, kA
);
```

To calculate the feedforward, simply call the `calculate()` method with the desired motor velocity and acceleration:

```java
// Calculates the feedforward for a position of 10 units,
// velocity of 20 units/second,
// and an acceleration of 30 units/second^2
// Units are determined by the units of the gains passed
// in at construction.
feedforward.calculate(10, 20, 30);
```

### Using Feedforward to Control a Mechanism

Feedforward control can be used entirely on its own, without a feedback controller. This is known as “open-loop” control, and for many mechanisms (especially robot drives) can be perfectly satisfactory. A `SimpleMotorFeedforward` might be employed to control a robot drive as follows:

```java
public void tankDriveWithFeedforward(double leftVelocity,
                                     double rightVelocity) {
  leftMotor.set(feedforward.calculate(leftVelocity));
  rightMotor.set(feedforward.calculate(rightVelocity));
}
```


# SquIDF

package com.seattlesolvers.solverslib.controller

A `SquIDF` controller is a variation of the `PIDF` controller that square roots the positional error term. Effectively, it extends the `PIDF` class, and overrides the `calculateOutput` method. This modification changes how the controller responds to errors of different magnitudes.

#### Key Differences from PIDF

| Aspect                    | PIDF                        | SquIDF               |
| ------------------------- | --------------------------- | -------------------- |
| **Proportional Response** | Linear with error           | Square root of error |
| **Large Errors**          | Stronger correction         | Moderate correction  |
| **Small Errors**          | Generally weaker correction | Stronger correction  |
| **Response Curve**        | Straight line               | Curved (sub-linear)  |

`SquIDF` helps reduces overshoot (since the square root dampens aggressive corrections when initial error is high), and generally gives a smoother approach. However, like a PID/PIDF, tuning is critical. The controller is only as good as you can tune it.

#### Usage Example

```java
// Create SquIDF controller with same gains as PIDF
SquIDFController squidfController = new SquIDFController(
    1.0,  // kP
    0.1,  // kI
    0.05, // kD
    0.2   // kF
);

// Use just like a PIDF controller
squidfController.setSetPoint(targetPosition);
double output = squidfController.calculate(currentPosition);
```

#### Tuning Notes

* **kP values** will typically need to be **higher** than in PIDF since √(e) < e for errors > 1
* Start with your PIDF gains and increase kP gradually
* The I, D, and F terms work pretty much identically to PIDF


# Utility Functions

package com.seattlesolvers.solverslib.solverslib.util;

SolversLib comes with many different Utility Functions:

* [Look Up Tables](https://docs.seattlesolvers.com/features/util#what-is-a-look-up-table)
* [Timing Functions](https://docs.seattlesolvers.com/features/util#timing-functions)
* [Math Utilities](https://docs.seattlesolvers.com/features/util#math-utilities)
* [Directional Enums](https://docs.seattlesolvers.com/features/util#directional-enums)

## What is a Look Up Table?

A look up table or LUT for short is used to store values and be able to quickly recall them.

The SolversLib provides 2 different variations of look up tables. In this year's game they can be used to store different set and tested velocities or angles. You can either retrieve the closest reference or you can interpolate through them.

## LUT (Look Up Table)

Provides a way to store values in a table to quickly retrieve them. For example, this might be used to store different speeds or angles based on certain distances. This class allows you to find the closest entry to the input.

For example if you enter:

| Input | Output |
| :---: | :----: |
|   0   |    0   |
|   1   |    1   |
|   2   |    1   |

When you request 1.1, it will return 1.

### Example Usage:

```java
import com.seattlesolvers.solverslib.util.LUT;

LUT<Double, Double> speeds = new LUT<Double, Double>()
{{
    add(5.0, 1.0);
    add(4.0, 0.9);
    add(3.0, 0.75);
    add(2.0, 0.5);
    add(1.0, 0.2);
}};

double distance = odometry.getPose().getTranslation().getDistance(new Translation2d(5, 10));
shooter.set(speeds.getClosest(distance));
```

## InterpLUT (Interpolated Look Up Table)

Provides a way to fill in the gaps in the data. Similarly to the LUT above, this allows you to add data points and retrieve a data point given an output. The difference between a normal LUT and InterpLUT is that the interpolated LUT uses math to fill in all the gaps. Effectively generating filler data based on the data around it.

### Example Usage:

```java
import com.seattlesolvers.solverslib.util.InterpLUT;

//Init the Look up table
InterpLUT lut = new InterpLUT();

//Adding each val with a key
lut.add(1.1, 0.2);
lut.add(2.7, .5);
lut.add(3.6, 0.75);
lut.add(4.1, 0.9);
lut.add(5, 1);
//generating final equation
lut.createLUT();

double distance = odometry.getPose().getTranslation().getDistance(new Translation2d(5, 10));
shooter.set(lut.get(distance));

//getting the velo required and passing it to the shooter.
```

## Timing Functions

SolversLib Comes with multiple timers and Timing Functions. They let you set the length, unit, can act as a stopwatch or even return the loop time.

### Timer

A timer can be created with a length or length and Time Unit. The various functions nested within the Timer object are:

| Function                | Return Type | Description                       |
| ----------------------- | ----------- | --------------------------------- |
| `timer.start()`         | Void        | Starts the Timer                  |
| `timer.pause()`         | Void        | Pauses the Timer                  |
| `timer.resume()`        | Void        | Resumes the Timer                 |
| `timer.elapsedTime()`   | long        | Returns the Passed Time           |
| `timer.remainingTime()` | long        | Returns Time Left                 |
| `timer.done()`          | Boolean     | Returns if the Timer is Completed |
| `timer.isTimerOn()`     | Boolean     | Returns if the Timer is Active    |

## Math Utilities

SolversLib currently adds 1 math utility, clamp. It lets you restrict a value to a certain max and min and is usable in double and int.

**Example Usage:**

Double Method:

```java
import com.seattlesolvers.solverslib.util;

double ValueToClamp;
double LowestPossibleValue;
double HighestPossibleValue;

double OutputVal = clamp(ValueToClamp,
                         LowestPossibleValue,
                         HighestPossibleValue);
```

Int Method:

```java
import com.seattlesolvers.solverslib.util;

int ValueToClamp;
int LowestPossibleValue;
int HighestPossibleValue;

int OutputVal = clamp(ValueToClamp,
                         LowestPossibleValue,
                         HighestPossibleValue);
```

## Directional Enums

SolversLib comes with multiple directional enums for all your directional needs! You can use these for any autonomous or TeleOP States or anything you want!

| Direction | Index |
| --------- | ----- |
| LEFT      | 0     |
| RIGHT     | 1     |
| UP        | 2     |
| DOWN      | 3     |
| FORWARD   | 4     |
| BACKWARDS | 5     |

You can find some examples in the [sample folder](https://github.com/FTC-23511/SolversLib/tree/master/examples/src/main/java/org/firstinspires/ftc/teamcode).


# Pedro Commands

package com.seattlesolvers.solverslib.pedroCommand

One of SolversLib's modern features is easy integration with [Pedro Pathing](< https://pedropathing.com/>), a popular path following library for Autonomous. To use this, make sure you have both the [pedroPathing module installed](https://docs.seattlesolvers.com/0.3.4/pedro-pathing/pages/pfxSCrt0ymNmrj0kRXOE#build.gradle-teamcode) as well as the [Pedro Pathing library installed](https://pedropathing.com/docs/pathing). This dependency is completely seperate from core, meaning that it is not installed by default, and should only be used if you are using Pedro Pathing.

SolversLib includes commands for using Pedro Pathing's Follower class, allowing you to fully use command base in your Autonomous OpModes.

These commands are for following Pedro Pathing's Path and PathChain classes, not SolversLib's Path class.&#x20;


# FollowPathCommand

import com.seattlesolvers.solverslib.pedroCommand.FollowPathCommand;

This command allows you to easily follow a Path or PathChain.

If a Path is supplied, it will simply convert it to a PatchChain first, and then follow that.

It has four parameters, with the first two being mandatory:

1. Pedro Pathing's Follower (which controls the robot movement)&#x20;
2. The Path or PathChain to follow

```java
new FollowPathCommand(follower, pathChain)
```

3. An optional boolean parameter called holdEnd that decides whether or not the robot should hold its position at the end of the Path (default value is true if not supplied)

```java
new FollowPathCommand(follower, pathChain, true)
```

4. An optional double parameter called maxPower that sets the maximum power the robot will run at for the path

```java
new FollowPathCommand(follower, pathChain, true, 0.5)
```

You can use a decorater to set the globalMaxPower for the follower as follows:

```java
new FollowPathCommand(follower, pathChain).setGlobalMaxPower(0.5)
```

Setting the Global Maximum Power sets the maximum power `globalMaxPower` for all future paths (unless rewritten again). However, setting the `maxPower` as a parameter in `FollowPathCommand` overwrites `globaMaxPower` for that path only.   &#x20;

To see how you can use this command in a [`CommandOpMode`](/0.3.4/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java). For usage in a full Autonomous Program, look at this [example](https://github.com/FTC-23511/SolversLib/blob/public-beta/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroAutoSample.java), and for a full TeleOp Program, at this [example](https://github.com/FTC-23511/SolversLib/blob/public-beta/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroTeleOpSample.java).


# HoldPointCommand

package com.seattlesolvers.solverslib.pedropathing

This command calls Pedro Pathing's [follower.holdPoint(Pose)](https://github.com/Pedro-Pathing/PedroPathing/blob/main/core/src/main/java/com/pedropathing/follower/Follower.java#L207), which allows you to easily hold a new Point (the Pose parameter is converted into a Point under the hood).

It has three mandatory parameters:

* Pedro Pathing's Follower (which controls the robot movement)
* The Pose to hold
* A boolean parameter called isFieldCentric that decides whether the move should be field centric or robot centric (based off the follower's position at the time of scheduling the command)

```java
HoldPointCommand(Follower follower, Pose pose, boolean isFieldCentric)
```

The following robot centric movements for Pedro Pathing's default coordinate system should be true assuming that the robot is facing forwards to the long side on the submersible's blue alliance side:

* `Pose.getX()`: -Y is forwards, +Y is backwards
* `Pose.getY()`: +X is left, -X is right
* `Pose.getHeading()`: Heading is in radians, +heading turns left and -heading turns right

To see how you can use this command in a [`CommandOpMode`](/0.3.4/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java).


# TurnCommand

package com.seattlesolvers.solverslib.pedropathing

This command calls Pedro Pathing's [follower.turn(radians, isLeft)](https://github.com/Pedro-Pathing/PedroPathing/blob/main/core/src/main/java/com/pedropathing/follower/Follower.java#L665), which allows you to easily turn a certain amount of radians (or degrees) in place.

It has four parameters, with the first three being mandatory:

1. Pedro Pathing's Follower (which controls the robot movement)
2. The angle to turn by (default is Radians)
3. Whether or not the robot should turn left (counterclockwise)

```java
TurnCommand(Follower follower, double angle, boolean isLeft)

// Example
new TurnCommand(follower, Math.PI / 2, true)
```

4. An optional parameter for a custom `AngleUnit` to turn by (`AngleUnit.Radians` or `AngleUnit.Degrees`)

```java
TurnCommand(Follower follower, double angle, boolean isLeft, AngleUnit angleUnit)

// Example
new TurnCommand(follower, 90.0, true, AngleUnit.DEGREES)
```

To see how you can use this command in a [`CommandOpMode`](/0.3.4/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java).


# TurnToCommand

package com.seattlesolvers.solverslib.pedropathing

This command calls Pedro Pathing's [follower.turnTo(radians)](https://github.com/Pedro-Pathing/PedroPathing/blob/main/core/src/main/java/com/pedropathing/follower/Follower.java#L675), which allows you to easily turn directly to a certain amount of radians (or degrees) in place.

It has three parameters, with the first two being mandatory:

1. Pedro Pathing's Follower (which controls the robot movement)
2. The angle to turn by (default is Radians)

```java
TurnToCommand(Follower follower, double angle)

// Example
new TurnToCommand(follower, Math.PI / 2)
```

3. An optional parameter for a custom `AngleUnit` to turn to (`AngleUnit.Radians` or `AngleUnit.Degrees`)

```java
TurnToCommand(Follower follower, double angle, AngleUnit angleUnit)

// Example
new TurnToCommand(follower, 90.0, AngleUnit.DEGREES)
```

To see how you can use this command in a [`CommandOpMode`](/0.3.4/command-base/command-system/robot-and-commandopmode#commandopmode), you can look at this [example](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PedroCommandSample/PedroCommands.java).


# What is Photon?

package com.seattlesolvers.solverslib.photon

Photon parallelizes hardware writes for servos and motors to make your robot's loop times faster. This is a stable version of the [now-archived PhotonCore](https://github.com/Eeshwar-Krishnan/PhotonFTC), that doesn't have probems.

{% hint style="warning" %}
Photon should be a last resort for improving loop times. Other traditional ways to help improve loop times can be found at <https://cookbook.dairy.foundation/improving_loop_times/improving_loop_times.html>.
{% endhint %}

#### How it Works

Normally with the SDK, after a setPower() or other LynxCommand, it waits for it to finish and check that it happened. But with Photon, after a setPower() or other LynxCommand, it immediately moves onto the next line of code/hardware write.

It is most effective with Swerve, because of the high amount of hardware writes frequently happening (4 servos + 4 motors).

#### Installation

See [installation page](https://docs.seattlesolvers.com/0.3.4/photon/pages/pfxSCrt0ymNmrj0kRXOE#build.gradle).

#### Usage

See [usage page](/0.3.4/photon/usage).


# Usage

package com.seattlesolvers.solverslib.photon

Photon only has 1 main requirement: you must use USB to connect hubs (USB 3.0 on Control Hub to USB mini b on expansion hub), NOT RS485. This is because Photon only works on USB-connected hubs.

{% hint style="danger" %}
**Warning:** Using USB instead of RS485 to communicate between hubs is not optional. You must use it for stable results, even if you are only running Photon on one hub.parallelized
{% endhint %}

{% hint style="info" %}
If you have a camera and wish to use Photon, it is recommended to use a USB 3.0 Hub to ensure that both the Expansion Hub and the camera is connected to the USB 3.0 port.&#x20;
{% endhint %}

To use in code with manual bulk caching:

```java
// Setup in your Robot class if you have one, or in init at start of opMode
// Don't do manual or auto bulk caching elsewhere - do it here.
PhotonCore.CONTROL_HUB.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
PhotonCore.EXPANSION_HUB.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
PhotonCore.experimental.setMaximumParallelCommands(8); // Can be adjusted based on user preference - but raising this number further can cause issues
PhotonCore.enable();
```

By default, both servos and motors will be parallelized. However, because Photon only works on USB-connected hubs, parallelizing servos will cause issues if you are powering servos using the REV Servo Hub, Servo Power Modules, or other non-USB devices. As such, you should disable `PARALLELIZE_SERVOS` when using non-USB devices to power servos.

```java
PhotonCore.PARALLELIZE_SERVOS = true; // Default set to true
PhotonCore.PARALLELIZE_SERVOS = false; // Set to false if using REV Servo Hub
```

{% hint style="info" %}
Using the goBILDA Servo Power Injector allows for full servo power (ideal for Swerve) while being Photon-compatible. This is because it directly uses the hub, and isn't a separate device.
{% endhint %}

Then, in every single run loop, you must clear the hub caches at the start (or end) of the loop to avoid getting stale values and get new data. This is the same for manual bulk reads as well.

```java
PhotonCore.CONTROL_HUB.clearBulkCache();
PhotonCore.EXPANSION_HUB.clearBulkCache();
```


# Odometry

package com.seattlesolvers.solverslib.kinematics

SolversLib offers its own odometry classes for the use of differential and holonomic drives. The odometry classes track the robot position as a `Pose2d`, which means it is represented using the vector $$\begin{pmatrix} x\ y\ \theta \end{pmatrix}$$ . $$x$$ is the distance in the forward direction of the robot, $$y$$ is the horizontal distance, and $$\theta$$ is the heading of the robot.

When using these classes, it is important to keep angles in radians. Distances should be consistent.

## Pose Exponential

Pose Exponential is a general FRC term for the constant velocity method of odometry utilized in FTC. We use pose exponentials because the cycle times of control loops in FTC can vary significantly.

This method uses differential equations to solve the nonlinear position of the robot given constant curvature. As such, we will not explain the math here. If you are interested in the math behind it, we suggest you read up on [Tyler's book](https://tavsys.net/controls-in-frc).

### Offsets and Trackwidth

The trackwidth is the distance between parallel encoders. This value should be tuned so that a precise calculation can be made. This value is a required pass into the constructor.

The center wheel offset accounts for the distance between the center of rotation of the robot and the position of the horizontal encoder. This is only necessary for the holonomic odometry.

To tune these values, make a rough estimate with a measured value and then use some sort of method for testing and updating the values until the outputs become reliable. Alternatively to beginning with a measured value, you can start at 18. Your value should then converge to some smaller distance.

## Creating the Odometry

A sample usage of SolversLib odometry can be found in this [sample folder](https://github.com/FTC-23511/SolversLib/tree/master/examples/src/main/java/org/firstinspires/ftc/teamcode/SharedOdometry). A sample for dead wheels can be found [here](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java).

### Using the Odometry Class

To use the odometry class, there are three different constructors depending on how you want to run your odometry. One method of running the odometry is using suppliers to have the class update your positions for you. The other is to manually input the sensor data into the `update()` method, which has parameters of the left encoder value, right encoder value, and horizontal encoder value (which is only used for holonomic). If you use suppliers, you can just call the `updatePose()` method which uses the suppliers to call the `update()` method.

```java
// define our trackwidth
static final double TRACKWIDTH = 13.7;

// convert ticks to inches
static final double TICKS_TO_INCHES = 15.3;

// create our encoders
MotorEx encoderLeft, encoderRight;
encoderLeft = new MotorEx(hardwareMap, "left_encoder");
encoderRight = new MotorEx(hardwareMap, "right_encoder");

// create our odometry
DifferentialOdometry diffOdom = new DifferentialOdometry(
    () -> encoderLeft.getCurrentPosition() * TICKS_TO_INCHES,
    () -> encoderRight.getCurrentPosition() * TICKS_TO_INCHES,
    TRACKWIDTH
);

// update the initial position
diffOdom.updatePose(new Pose2d(1, 2, 0));

// control loop
while (!isStopRequested()) {
    /* implementation */

    // update the position
    diffOdom.updatePose();
}
```

You should call the respective update method once every cycle of the control loop.

### Using the Odometry Subsystem

The [OdometrySubsystem](https://github.com/FTC-23511/SolversLib/blob/master/core/src/main/java/com/seattlesolvers/solverslib/command/OdometrySubsystem.java) class is a template subsystem meant to make command-based programming around odometry much simpler and functional. Using the odometry subsystem makes it more accurate because the position will update itself through the scheduler's call to its `periodic()` method, which updates the position. The subsystem makes use of the suppliers, so you will **need** to use that constructor instead of the other for the SolversLib subsystem. Alternatively, you can create your own odometry subsystem.

```java
// define our constants
static final double TRACKWIDTH = 13.7;
static final double TICKS_TO_INCHES = 15.3;
static final double CENTER_WHEEL_OFFSET = 2.4;

// create our encoders
MotorEx encoderLeft, encoderRight, encoderPerp;
encoderLeft = new MotorEx(hardwareMap, "left_encoder");
encoderRight = new MotorEx(hardwareMap, "right_encoder");
encoderPerp = new MotorEx(hardwareMap, "center_encoder");

encoderLeft.setDistancePerPulse(TICKS_TO_INCHES);
encoderRight.setDistancePerPulse(TICKS_TO_INCHES);
encoderPerp.setDistancePerPulse(TICKS_TO_INCHES);

// create the odometry object
HolonomicOdometry holOdom = new HolonomicOdometry(
    encoderLeft::getDistance,
    encoderRight::getDistance,
    encoderPerp::getDistance,
    TRACKWIDTH, CENTER_WHEEL_OFFSET
);

// create the odometry subsystem
OdometrySubsystem odometry = new OdometrySubsystem(holOdom);
```

The [PurePursuitCommand](https://docs.seattlesolvers.com/pathing/pure-pursuit#using-the-pure-pursuit-command) makes use of the OdometrySubsystem class.


# WPILib Kinematics

package com.seattlesolvers.solverslib.kinematics.wpilibkinematics

Check out the [WPILib documentation](https://docs.wpilib.org/en/latest/docs/software/kinematics-and-odometry/index.html).

## What are Kinematics?

The kinematics suite contains classes for differential drive, swerve drive, and mecanum drive kinematics and odometry. The kinematics classes help convert between a universal `ChassisSpeeds` object, containing linear and angular velocities for a robot to usable speeds for each individual type of [drivetrain](/0.3.4/features/drivebases) i.e. left and right wheel speeds for a differential drive, four wheel speeds for a mecanum drive, or individual module states (speed and angle) for a swerve drive.

## What is Odometry?

Odometry involves using sensors on the robot to create an estimate of the position of the robot on the field. In FTC, these sensors are typically several encoders (the exact number depends on the drive type) and an optional gyroscope to measure robot angle. The odometry classes utilize the kinematics classes along with periodic user inputs about speeds (and angles in the case of swerve) to create an estimate of the robot’s location on the field. This odometry is a means of working around the odometer system that is often used by FTC teams.

## The `ChassisSpeeds` Class

The `ChassisSpeeds` object is essential to the WPILib kinematics and odometry suite. The `ChassisSpeeds` object represents the speeds of a robot chassis. This struct has three components:

* `vx`: The velocity of the robot in the x (forward) direction.
* `vy`: The velocity of the robot in the y (sideways) direction. (Positive values mean the robot is moving to the left).
* `omega`: The angular velocity of the robot in radians per second.

A non-holonomic drivetrain (i.e. a drivetrain that cannot move sideways, ex: a differential drive) will have a `vy` component of zero because of its inability to move sideways.

### Constructing a ChassisSpeeds Object

The constructor for the `ChassisSpeeds` object is very straightforward, accepting three arguments for `vx`, `vy`, and `omega`. In Java, `vx` and `vy` must be in meters per second. In C++, the units library may be used to provide a linear velocity using any linear velocity unit.

```java
// The robot is moving at 3 meters per second forward, 2 meters
// per second to the right, and rotating at half a rotation per
// second counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(3.0, -2.0, Math.PI);
```

### Creating a ChassisSpeeds Object from Field-Relative Speeds

A `ChassisSpeeds` object can also be created from a set of field-relative speeds when the robot angle is given. This converts a set of desired velocities relative to the field (for example, toward the opposite alliance station and toward the right field boundary) to a `ChassisSpeeds` object which represents speeds that are relative to the robot frame. This is useful for implementing field-oriented controls for a swerve or mecanum drive robot.

The static `ChassisSpeeds.fromFieldRelativeSpeeds()` method can be used to generate the `ChassisSpeeds` object from field-relative speeds. This method accepts the `vx` (relative to the field), `vy` (relative to the field), `omega`, and the robot angle.

```java
// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise. The current
// robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds
(
    2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);
```

The angular velocity is not explicitly stated to be “relative to the field” because the angular velocity is the same as measured from a field perspective or a robot perspective.


# Old Kinematics

Swerve Drive Kinematics have been rewritten, and hence, the old Kinematics classes have been Deprecated. A reference for these older classes can be found in the subpages of this page.


# Swerve Drive Odometry

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.SwerveDriveOdometry

A user can use the swerve drive kinematics classes in order to perform [odometry](/0.3.4/kinematics/wpilib-kinematics#what-is-odometry). WPILib/SolversLib contains a `SwerveDriveOdometry` class that can be used to track the position of a swerve drive robot on the field.

**Note**:\
Because this method only uses encoders and a gyro, the estimate of the robot’s position on the field will drift over time, especially as your robot comes into contact with other robots during gameplay. However, odometry is usually very accurate during the autonomous period.

## Creating the Odometry Object

The `SwerveDriveOdometry` class requires two mandatory arguments, and one optional argument. The mandatory arguments are the kinematics object that represents your swerve drive (in the form of a `SwerveDriveKinematics` class) and the angle reported by your gyroscope (as a Rotation2d). The third optional argument is the starting pose of your robot on the field (as a `Pose2d`). By default, the robot will start at $$\begin{pmatrix} x\ y\ \theta \end{pmatrix} = \begin{pmatrix} 0\ 0\ 0 \end{pmatrix}$$ .

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase.

```java
// Locations for the swerve drive modules relative to the
// robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);

// Creating my odometry object from the kinematics object. Here,
// our starting pose is 5 meters along the long end of the
// field and in the
// center of the field along the short end, facing forward.
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry
(
  m_kinematics, getGyroHeading(),
  new Pose2d(5.0, 13.5, new Rotation2d()
);
```

## Updating the Robot Pose

The `update` method of the odometry class updates the robot position on the field. The update method takes in the gyro angle of the robot, along with a series of module states (speeds and angles) in the form of a `SwerveModuleState` each. It is important that the order in which you pass the `SwerveModuleState` objects is the same as the order in which you created the kinematics object.

This `update` method must be called periodically, preferably in the `periodic()` method of a [Subsystem](/0.3.4/command-base/command-system/subsystems). The `update` method returns the new updated pose of the robot.

```java
@Override
public void periodic() {
  // Get my gyro angle.
  Rotation2d gyroAngle = Rotation2d.fromDegrees(m_gyro.getHeading());

  // Update the pose
  m_pose = m_odometry.update
  (
    gyroAngle, m_frontLeftModule.getState(),
    m_frontRightModule.getState(),
    m_backLeftModule.getState(), m_backRightModule.getState()
  );
}
```

### Resetting the Robot Pose

The robot pose can be reset via the `resetPose` method. This method accepts two arguments – the new field-relative pose and the current gyro angle.

If at any time, you decide to reset your gyroscope, the `resetPose` method MUST be called with the new gyro angle.

The implementation of `getState()` above is left to the user. The idea is to get the module state (speed and angle) from each module.

In addition, the `getPoseMeters()` method can be used to retrieve the current robot pose without an update.


# Swerve Drive Kinematics

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.SwerveDriveKinematics

The `SwerveDriveKinematics` class is a useful tool that converts between a `ChassisSpeeds` object and several `SwerveModuleState` objects, which contains velocities and angles for each swerve module of a swerve drive robot.

## The `SwerveModuleState` Class

The `SwerveModuleState` class contains information about the velocity and angle of a singular module of a swerve drive. The constructor for a `SwerveModuleState` takes in two arguments, the velocity of the wheel on the module, and the angle of the module.

The velocity of the wheel must be in meters per second. An angle of 0 from the module represents the forward-facing direction.

## Constructing the Kinematics Object

The `SwerveDriveKinematics` class accepts a variable number of constructor arguments, with each argument being the location of a swerve module relative to the robot center (as a `Translation2d`. The number of constructor arguments corresponds to the number of swerve modules. A swerve bot must have AT LEAST two swerve modules.

The locations for the modules must be relative to the center of the robot. Positive x values represent moving toward the front of the robot whereas positive y values represent moving toward the left of the robot.

```java
// Locations for the swerve drive modules
// relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);
```

## Converting Chassis Speeds to Module States

The `toSwerveModuleStates(ChassisSpeeds speeds)` method should be used to convert a `ChassisSpeeds` object to a an array of `SwerveModuleState` objects. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual module states.

The elements in the array that is returned by this method are the same order in which the kinematics object was constructed. For example, if the kinematics object was constructed with the front left module location, front right module location, back left module location, and the back right module location in that order, the elements in the array would be the front left module state, front right module state, back left module state, and back right module state in that order.

```java
// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to module states
SwerveModuleState[] moduleStates =
    kinematics.toSwerveModuleStates(speeds);

// Front left module state
SwerveModuleState frontLeft = moduleStates[0];

// Front right module state
SwerveModuleState frontRight = moduleStates[1];

// Back left module state
SwerveModuleState backLeft = moduleStates[2];

// Back right module state
SwerveModuleState backRight = moduleStates[3];
```

### Field-Oriented Drive

[Recall](/0.3.4/kinematics/wpilib-kinematics) that a `ChassisSpeeds` object can be created from a set of desired field-oriented speeds. This feature can be used to get module states from a set of desired field-oriented speeds.

```java
// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise.
// The current robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);

// Now use this in our kinematics
SwerveModuleState[] moduleStates =
  kinematics.toSwerveModuleStates(speeds);
```

### Using Custom Centers of Rotation

Sometimes, rotating around one specific corner might be desirable for certain evasive maneuvers. This type of behavior is also supported by the WPILib classes. The same `ToSwerveModuleStates()` method accepts a second parameter for the center of rotation (as a `Translation2d`). Just like the wheel locations, the `Translation2d` representing the center of rotation should be relative to the robot center.

Because all robots are a rigid frame, the provided `vx` and `vy` velocities from the `ChassisSpeeds` object will still apply for the entirety of the robot. However, the `omega` from the `ChassisSpeeds` object will be measured from the center of rotation.

For example, one can set the center of rotation on a certain module and if the provided `ChassisSpeeds` object has a `vx` and `vy` of zero and a non-zero `omega`, the robot will appear to rotate around that particular swerve module.

## Converting Module States to Chassis Speeds

One can also use the kinematics object to convert an array of `SwerveModuleState` objects to a singular `ChassisSpeeds` object. The `toChassisSpeeds(SwerveModuleState... states)` method can be used to achieve this.

```java
// Example module states
SwerveModuleState frontLeftState =
  new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleState frontRightState =
  new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleState backLeftState =
  new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleState backRightState =
  new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(
  frontLeftState, frontRightState, backLeftState, backRightState
);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;
```


# Differential Drive Kinematics

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.DifferentialDriveKinematics

The `DifferentialDriveKinematics` class is a useful tool that converts between a `ChassisSpeeds` object and a `DifferentialDriveWheelSpeeds` object, which contains velocities for the left and right sides of a differential drive robot.

## Constructing the Kinematics Object

The `DifferentialDriveKinematics` object accepts one constructor argument, which is the trackwidth of the robot. This represents the distance between the two sets of wheels on a differential drive.

**Note**: The trackwidth must be in meters.

## Converting Chassis Speeds to Wheel Speeds

The `toWheelSpeeds(ChassisSpeeds speeds)` method should be used to convert a `ChassisSpeeds` object to a `DifferentialDriveWheelSpeeds` object. This is useful in situations where you have to convert a linear velocity (`vx`) and an angular velocity (`omega`) to left and right wheel velocities.

```java
// Creating my kinematics object: track width of 15 inches
DifferentialDriveKinematics kinematics =
  new DifferentialDriveKinematics(15.0 / 254.0);

// Example chassis speeds: 2 meters per second linear velocity,
// 1 radian per second angular velocity.
ChassisSpeeds chassisSpeeds = new ChassisSpeeds(2.0, 0, 1.0);

// Convert to wheel speeds
DifferentialDriveWheelSpeeds wheelSpeeds =
  kinematics.toWheelSpeeds(chassisSpeeds);

// Left velocity
double leftVelocity = wheelSpeeds.leftMetersPerSecond;

// Right velocity
double rightVelocity = wheelSpeeds.rightMetersPerSecond;
```

## Converting Wheel Speeds to Chassis Speeds

One can also use the kinematics object to convert individual wheel speeds (left and right) to a singular `ChassisSpeeds` object. The `toChassisSpeeds(DifferentialDriveWheelSpeeds speeds)` method should be used to achieve this.

```java
// Creating my kinematics object: track width of 15 inches
DifferentialDriveKinematics kinematics =
  new DifferentialDriveKinematics(15.0 / 254.0);

// Example differential drive wheel speeds: 2 meters per second
// for the left side, 3 meters per second for the right side.
DifferentialDriveWheelSpeeds wheelSpeeds =
  new DifferentialDriveWheelSpeeds(2.0, 3.0);

// Convert to chassis speeds.
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds);

// Linear velocity
double linearVelocity = chassisSpeeds.vxMetersPerSecond;

// Angular velocity
double angularVelocity = chassisSpeeds.omegaRadiansPerSecond;
```


# Differential Drive Odometry

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.DifferentialDriveOdometry

A user can use the differential drive kinematics classes in order to perform [odometry](/0.3.4/kinematics/wpilib-kinematics#what-is-odometry). WPILib/SolversLib contains a `DifferentialDriveOdometry` class that can be used to track the position of a differential drive robot on the field.

**Note**:\
Because this method only uses encoders and a gyro, the estimate of the robot’s position on the field will drift over time, especially as your robot comes into contact with other robots during gameplay. However, odometry is usually very accurate during the autonomous period.

## Creating the Odometry Object

The `DifferentialDriveOdometry` class requires one mandatory argument and one optional argument. The mandatory argument is the angle reported by your gyroscope (as a Rotation2d). The optional argument is the starting pose of your robot on the field (as a `Pose2d`). By default, the robot will start at $$\begin{pmatrix} x\ y\ \theta \end{pmatrix} = \begin{pmatrix} 0\ 0\ 0 \end{pmatrix}$$ .

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase.

The encoder positions **must** be reset to zero before constructing the `DifferentialDriveOdometry` class.

```java
// Creating my odometry object. Here,
// our starting pose is 5 meters along the long end of the field
// and in the center of the field along the short end,
// facing forward.
DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry
(
    getGyroHeading(), new Pose2d(5.0, 13.5, new Rotation2d()
);
```

## Updating the Robot Pose

The `update` method can be used to update the robot’s position on the field. This method must be called periodically, preferably in the `periodic()` method of a [Subsystem](/0.3.4/command-base/command-system/subsystems). The `update` method returns the new updated pose of the robot. This method takes in the gyro angle of the robot, along with the left encoder distance and right encoder distance.

Ensure your encoder distances are in meters!

### Resetting the Robot Pose

The robot pose can be reset via the `resetPose` method. This method accepts two arguments – the new field-relative pose and the current gyro angle.

If at any time, you decide to reset your gyroscope, the `resetPose` method MUST be called with the new gyro angle. Furthermore, the encoders must also be reset to zero when resetting the pose.


# Mecanum Drive Kinematics

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.MecanumDriveKinematics

The `MecanumDriveKinematics` class is a useful tool that converts between a `ChassisSpeeds` object and a `MecanumDriveWheelSpeeds` object, which contains velocities for each of the four wheels on a mecanum drive.

## Constructing the Kinematics Object

The `MecanumDriveKinematics` class accepts four constructor arguments, with each argument being the location of a wheel relative to the robot center (as a `Translation2d`). The order for the arguments is front left, front right, back left, and back right. The locations for the wheels must be relative to the center of the robot. Positive x values represent moving toward the front of the robot whereas positive y values represent moving toward the left of the robot.

```java
// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);
```

## Converting Chassis Speeds to Wheel Speeds

The `toWheelSpeeds(ChassisSpeeds speeds)` method should be used to convert a `ChassisSpeeds` object to a `MecanumDriveWheelSpeeds` object. This is useful in situations where you have to convert a forward velocity, sideways velocity, and an angular velocity into individual wheel speeds.

```java
// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds =
    kinematics.toWheelSpeeds(speeds);

// Get the individual wheel speeds
double frontLeft = wheelSpeeds.frontLeftMetersPerSecond;
double frontRight = wheelSpeeds.frontRightMetersPerSecond;
double backLeft = wheelSpeeds.rearLeftMetersPerSecond;
double backRight = wheelSpeeds.rearRightMetersPerSecond;
```

### Field-Oriented Drive

[Recall](/0.3.4/kinematics/wpilib-kinematics) that a `ChassisSpeeds` object can be created from a set of desired field-oriented speeds. This feature can be used to get wheel speeds from a set of desired field-oriented speeds.

```java
// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise.
// The current robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0)
);

// Now use this in our kinematics
MecanumDriveWheelSpeeds wheelSpeeds =
  kinematics.toWheelSpeeds(speeds);
```

### Using Custom Centers of Rotation

Sometimes, rotating around one specific corner might be desirable for certain evasive maneuvers. This type of behavior is also supported by the WPILib classes. The same `toWheelSpeeds()` method accepts a second parameter for the center of rotation (as a `Translation2d`). Just like the wheel locations, the `Translation2d` representing the center of rotation should be relative to the robot center.

Because all robots are a rigid frame, the provided `vx` and `vy` velocities from the `ChassisSpeeds` object will still apply for the entirety of the robot. However, the `omega` from the `ChassisSpeeds` object will be measured from the center of rotation.

For example, one can set the center of rotation on a certain wheel and if the provided `ChassisSpeeds` object has a `vx` and `vy` of zero and a non-zero `omega`, the robot will appear to rotate around that particular wheel.

## Converting Wheel Speeds to Chassis speeds

One can also use the kinematics object to convert a `MecanumDriveWheelSpeeds` object to a singular `ChassisSpeeds` object. The `toChassisSpeeds(MecanumDriveWheelSpeeds speeds)` method can be used to achieve this.

```java
// Example wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds =
    new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds =
    kinematics.toChassisSpeeds(wheelSpeeds);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;
```


# Mecanum Drive Odometry

import com.seattlesolvers.solverslib.kinematics.wpilibkinematics.MecanumDriveOdometry

A user can use the mecanum drive kinematics classes in order to perform [odometry](/0.3.4/kinematics/wpilib-kinematics#what-is-odometry). WPILib/SolversLib contains a `MecanumDriveOdometry` class that can be used to track the position of a mecanum drive robot on the field.

**Note**:\
Because this method only uses encoders and a gyro, the estimate of the robot’s position on the field will drift over time, especially as your robot comes into contact with other robots during gameplay. However, odometry is usually very accurate during the autonomous period.

## Creating the Odometry Object

The `MecanumDriveOdometry` class requires two mandatory arguments and one optional argument. The mandatory arguments are the kinematics object that represents your mecanum drive (in the form of a `MecanumDriveKinematics` class) and the angle reported by your gyroscope (as a Rotation2d). The third optional argument is the starting pose of your robot on the field (as a `Pose2d`). By default, the robot will start at $$\begin{pmatrix} x\ y\ \theta \end{pmatrix} = \begin{pmatrix} 0\ 0\ 0 \end{pmatrix}$$ .

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase.

```java
// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation =
  new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation =
  new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation =
  new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation =
  new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics
(
  m_frontLeftLocation, m_frontRightLocation,
  m_backLeftLocation, m_backRightLocation
);

// Creating my odometry object from the kinematics object. Here,
// our starting pose is 5 meters along the long end of the field and in the
// center of the field along the short end, facing forward.
MecanumDriveOdometry m_odometry = new MecanumDriveOdometry
(
  m_kinematics, getGyroHeading(),
  new Pose2d(5.0, 13.5, new Rotation2d()
);
```

## Updating the Robot Pose

The `update` method of the odometry class updates the robot position on the field. The update method takes in the gyro angle of the robot, along with a `MecanumDriveWheelSpeeds` object representing the speed of each of the 4 wheels on the robot. This `update` method must be called periodically, preferably in the `periodic()` method of a [Subsystem](/0.3.4/command-base/command-system/subsystems). The `update` method returns the new updated pose of the robot.

The `MecanumDriveWheelSpeeds` class must be constructed with each wheel speed in meters per second.

```java
@Override
public void periodic() {
  // Get my wheel speeds; assume .getRate() has been
  // set up to return velocity of the encoder
  // in meters per second.
  MecanumDriveWheelSpeeds wheelSpeeds = new MecanumDriveWheelSpeeds
  (
      m_frontLeftEncoder.getRate(), m_frontRightEncoder.getRate(),
      m_backLeftEncoder.getRate(), m_backRightEncoder.getRate()
  );

  // Get my gyro angle.
  Rotation2d gyroAngle = Rotation2d.fromDegrees(m_gyro.getAngle());

  // Update the pose
  m_pose = m_odometry.update(gyroAngle, wheelSpeeds);
}
```

### Resetting the Robot Pose

The robot pose can be reset via the `resetPose` method. This method accepts two arguments – the new field-relative pose and the current gyro angle.

If at any time, you decide to reset your gyroscope, the `resetPose` method MUST be called with the new gyro angle.

In addition, the `getPoseMeters()` method can be used to retrieve the current robot pose without an update.


# Point-to-Point

package com.seattlesolvers.solverslib.p2p

SolversLib Point-to-Point (p2p) for Swerve is still a WIP, and full documentation is coming soon!

The full source code is in SolversLib's p2p directory, and can be viewed [here](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/p2p).

You can check out the beta visualizer for SolversLib p2p at <https://visualizer.seattlesolvers.com/>.


# Pure Pursuit

package com.seattlesolvers.solverslib.purepursuit

## SolversLib Pure Pursuit is buggy, and has been replaced with [Point-to-Point](/0.3.4/pathing/point-to-point). Its use is currently not recommend.

## Pure Pursuit

The pure pursuit algorithm in SolversLib is developed so that the user only needs to add the desired waypoints and call the `followPath()` method in the [Path](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/purepursuit/Path.java) class. To use this, you need to pass the [mecanum](https://docs.SolversLib.org/SolversLib/features/drivebases#mecanum) drivetrain as well as the odometry for the robot. Once the method is finished, it will return true or false depending on if it was successful or not.

As an alternative, you can call the `loop()` method and directly input your odometry positions there. Make sure you update the odometry positions with each iteration of the loop.

### What is Pure Pursuit?

Pure pursuit, otherwise designated as "PP," is a path tracking algorithm that calculates the robot velocity in order to reach a designated look-ahead point from the current position. It loosely follows a path determined by a set of waypoints, which are coordinates on the field. What the pure pursuit controller does is create a circle of determined radius and follow the path by "looking ahead" with the circle and seeing where it intersects with the path. The robot's heading orientation is then compared to the radius that connects the center of the robot to that intersection. It then moves in correspondence. The radius size can be updated for each waypoint you enter into the path for specificity.

![A visual representation of look-ahead](/files/ompp5AF5xM4zG3U2r2eg)

The robot continues to follow this intersection at real-time. This is how the robot "follows" the designated path. It is essentially a p controller for the heading that has the robot move at the fastest possible speed around some path.

#### Waypoints

There are five types of waypoints: start, general, interrupted, point-turn, and end. The starting waypoint represents the first point in the path; conversely, the ending waypoint is the last point in the path.

A general waypoint is a point where the robot performs its ordinary pursuit algorithm with the look-ahead method. A point-turn waypoint is a type of general waypoint that stops at the given point, turns, and then traverses to the next waypoint. An interrupted waypoint is a type of point-turn waypoint where other actions can occur, such as picking up a skystone.

**Creating Waypoints**

You can create a waypoint by calling the various constructors.

**StartWaypoint**

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p1 = new StartWaypoint();

// With Pose2d.
Waypoint p1 = new StartWaypoint(pose2d);
// With X and Y coordinates.
Waypoint p1 = new StartWaypoint(x, y);
// With Translation2d.
Waypoint p1 = new StartWaypoint(translation2d);
```

**GeneralWaypoint**

Each general waypoint inherits from the previous general waypoint in the path. The only parameters that will need to be specified are the x and y coordinates of the point. Each point has the option to update the different parameters across the path (which is meant for user-end customization of the path). Point-turn waypoints, interrupted waypoints, and end waypoints are all subclasses of a general waypoint, so they will also have this feature. This inheritance is performed in the `init()` method of the path.

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p2 = new GeneralWaypoint();

// With X and Y coordinates. This waypoint will inherit
// its settings from the previous waypoint.
// Useful for long strings of waypoints.
// Note: Will not work if the waypoint preceding
// this is not an instance of GeneralWaypoint. 
Waypoint p2 = new GeneralWaypoint(x, y);

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p2 = new GeneralWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius
);
// With Pose2D.
Waypoint p2 = new GeneralWaypoint(
    pose2d, movementSpeed, turnSpeed,
    followRadius
);
// With X and Y coordinates.
Waypoint p2 = new GeneralWaypoint(
    x, y, movementSpeed,
    turnSpeed, followRadius
);
// With a preferred angle.
Waypoint p2 = new GeneralWaypoint(
    x, y, rotationRadians,    
    movementSpeed, turnSpeed,
    followRadius
);
```

**PointTurnWaypoint**

As you will see here, a "buffer" is a sort of expected error. This sets up a tolerance given that the robot might be a bit offset from the desired position or rotation.

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p3 = new PointTurnWaypoint();

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p3 = new PointTurnWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer
);
// With Pose2D.
Waypoint p3 = new PointTurnWaypoint(
    pose2d, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
// With X and Y coordinates and preferred angle.
Waypoint p3 = new PointTurnWaypoint(
    x, y, rotationRadians, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
// With X and Y coordinates.
Waypoint p3 = new PointTurnWaypoint(
    x, y, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
```

**InterruptWaypoint**

The `action` here is an [InterruptAction](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/purepursuit/actions/InterruptAction.java), which is an interface that the user can implement to create a custom action to occur at this point. A recommendation is to pair this with the [command paradigm](/0.3.4/command-base/command-system) that SolversLib provides.

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p4 = new InterruptWaypoint();

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p4 = new InterruptWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, action
);
// With Pose2D.
Waypoint p4 = new InterruptWaypoint(
    pose2d, movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, action
);
// With X and Y coordinates and preferred angle.
Waypoint p4 = new InterruptWaypoint(
    x, y, rotationRadians,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, action
);
// With X and Y coordinates.
Waypoint p4 = new InterruptWaypoint(
    x, y, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer,
    action
);

// With java 8 you can use a lambda expression to easily
// add an action. For example:
Waypoint p4 = new InterruptWaypoint(
    x, y, movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer, grabSubsystem::grabBlock
);
```

**EndWaypoint**

```java
// Empty constructor. Note: Only use this constructor
// if you plan on setting the values later.
Waypoint p5 = new EndWaypoint();

/**
 * Normal constructors.
 */
// With Translation2d and Rotation2d.
Waypoint p5 = new PointTurnWaypoint(
    translation2d, rotation2d,
    movementSpeed, turnSpeed,
    followRadius, positionBuffer,
    rotationBuffer
);
// With Pose2D.
Waypoint p5 = new PointTurnWaypoint(
    pose2d, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
// With X and Y coordinates and preferred angle
// (A preferred angle is needed for an EndWaypoint).
Waypoint p5 = new PointTurnWaypoint(
    x, y, rotationRadians, movementSpeed,
    turnSpeed, followRadius,
    positionBuffer, rotationBuffer
);
```

### Creating the Path

Before you can call the `loop()` or `followPath()` method, you need to follow the proper procedure. If you are using `loop()`, you need to call the `init()` method to ensure your path is legal and set up the unconfigured waypoints.

```java
// we are using the waypoints we made in the above examples
Path m_path = new Path(p1, p2, p3, p4, p5);

m_path.init(); // initialize the path
```

If the path is not legal, an exception will be thrown.

#### Intersections

An intersection is the point where the follow distance represented by a circle around the robot meets the drawn path derived from the waypoints. The "best intersection" is determined by either waypoint order or heading. This intersection point where the circle meets the path is where the robot will move to. The pure pursuit algorithm determines the best intersection and calculates the motor powers needed to reach said position. This updates with each loop, so the intersection point can change with each step due to the movement of the robot. While the conventional pure pursuit algorithm used heading controlled waypoints, SolversLib features a custom type of intersection control we call "order controlled". This type of control is more powerful and less prone to errors then heading controlled and is enabled by default. If you wish to use heading controlled instead, use this (not recommended):

```java
m_path.setPathType(PathType.HEADING_CONTROLLED);
```

#### Retrace

SolversLib's pure pursuit implementation includes a unique feature we call retrace. One common issue with pure pursuit is that the robot can lose it's path. Retrace solves this issue. If enabled (retrace is enabled by default) and the robot loses it's path, the software will automatically plot a temporary path back to it's last known path position. Once the robot finds the path again it will continue on as normal. If you wish to disable retrace (not recommended), do this:

```java
m_path.disableRetrace();
```

#### Timeouts

Advanced teams may want to have more control over how long the robot get to have to complete a path. If the robot is stuck on a path/waypoint for too long, you may want to stop the path to avoid accidental penalties. To set timeouts do the following:

```java
// For an entire path
m_path.setWaypointTimeouts(timeout);

// For individual waypoints
m_path.setWaypointTimeouts(p1_timeout, p2_timeout, p3_timeout, ...);

// Reset timeouts.
m_path.resetTimeouts();
```

#### Resetting the Path

If you want to use a path more than once in the same opmode, make sure to reset between uses. You can do this as follows:

```java
m_path.reset();
```

### Using `followPath()`

The `followPath()` method is the automatic implementation of pure pursuit for SolversLib. For teams that want to use all of SolversLib's features to the fullest, this is the recommended process.

An important note for the pure pursuit algorithm is that it only works well with odometry. You can use the various odometry systems provided by SolversLib. An important thing to note is that `followPath()` makes use of the [Odometry](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/kinematics/Odometry.java) abstract class and the [mecanum drivebase](https://docs.SolversLib.org/SolversLib/features/drivebases#mecanum). Then, the method will call the loop method and do everything for you.

```java
// follow path
m_path.followPath(m_robotDrive, m_robotOdometry);
```

An issue this method has is that we cannot directly access the hardware of the robot. As a result, the `update()` method for the odometry is not called in `followPath()`. Instead, we call `updatePose()`.

#### Creating Your Odometry

As a way of working around this issue, the odometry needs to be setup in a particular way with [Suppliers](https://docs.oracle.com/javase/8/docs/api/java/util/function/Supplier.html). A supplier is a functional interface that uses lambdas to reference a certain value. Let's work with the [HolonomicOdometry](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/kinematics/HolonomicOdometry.java) class for these examples.

You're going to want to instantiate your odometry using this constructor:

```java
HolonomicOdometry m_robotOdometry = new HolonomicOdometry(
    leftValue, rightValue, horizontalValue, trackWidth,
    centerWheelOffset
);
```

**Creating the Suppliers**

Before we can create the object, we need to make our suppliers. We will do this by using the [DoubleSupplier](https://docs.oracle.com/javase/8/docs/api/java/util/function/DoubleSupplier.html) implementation. We need to create three of these objects: one for each odometer. Let's assume the user has created a method that automatically converts ticks to inches for an external encoder.

```java
DoubleSupplier leftValue, rightValue, horizontalValue;

leftValue = () -> ticksToInches(m_lOdom.getCurrentPosition());
rightValue = () -> ticksToInches(m_rOdom.getCurrentPosition());
horizontalValue = () -> ticksToInches(m_hOdom.getCurrentPosition());
```

### Using `loop()`

The alternative to the `followPath()` method is the `loop()` method. For teams that want to use solely the SolversLib implementation of pure pursuit and perform the rest of the actions themselves, then this is the more appealing method.

The use of suppliers can be avoided using this method since it can be called in your own class with access to the hardware directly.

#### Odometry Options

The odometry is much more open for this. You can use whatever constructor you desire for it. Since the method parameters only take x, y, and heading values, you can use whatever odometry system you desire as long as it produces such values. This is one of the more appealing aspects of the `loop()` method.

The important thing for odometry is to remember to update the position of the robot after each iteration after manually inputting the motor speeds.

#### Calling the Method

This is the principle path method. After everything is configured and initiated, this method can be used. Using the robot's x, y, and rotation, this method calculates the appropriate motor powers for the robot to follow the path. This method calls all triggered/interrupted actions automatically. If this returns zero motor speeds `{0, 0, 0}` that means the path has either (1) timed out, (2) lost the path and retrace was disabled, or (3) reached the destination. Use `isFinished()` and `timedOut()` to troubleshoot.

Below is an example using a custom robot class that includes the drivebase and odometry:

```java
while (!m_path.isFinished()) {
    if (m_path.timedOut())
        throw new InterruptedException("Timed out");

    // return the motor speeds
    double speeds[] = m_path.loop(m_robot.getX(), m_robot.getY(),
                                  m_robot.getHeading());

    m_robot.drive(speeds[0], speeds[1], speeds[2]);
    m_robot.updatePose();
}
m_robot.stop();
```

### Using the Pure Pursuit Command

If you're using your odometry for multiple subsystems, you're likely going to want to make use of the [PurePursuitCommand](https://github.com/SolversLib/SolversLib/blob/v2.1.1/core/src/main/java/com/arcrobotics/SolversLib/command/PurePursuitCommand.java) due to the shared odometry (as we only want to update it once per cycle). This is actually the recommended method of using pure pursuit, especially if you want to use it with the command-based paradigm that SolversLib has to offer.

#### Creating an Odometry Subsystem

The pre-built PurePursuitCommand requires the use of SolversLib's [OdometrySubsystem](https://docs.SolversLib.org/SolversLib/kinematics/odometry#using-the-odometry-subsystem). It is fairly easy to set up. All that is needed is for the user to pass in their odometry class into the constructor of the subsystem.

```java
// create the odometry object
HolonomicOdometry m_robotOdometry = new HolonomicOdometry(
    leftValue, rightValue, horizontalValue, TRACKWIDTH,
    CENTER_WHEEL_OFFSET
);

// pass the odometry object into the subsystem constructor
OdometrySubsystem m_odometry = new OdometrySubsystem(
    m_robotOdometry
);
```

**Using the Odometry Subsystem**

Users can make use of the odometry subsystem in the exact same way as the Odometry class.

```java
// retrieve the current saved pose of the robot
Pose2d robotPose = m_odometry.getPose();

// update the pose manually
m_odometry.update();
```

The odometry subsystem updates the position of the robot in its `periodic()` method, so it will update every time the CommandScheduler is run. This effectively means that the position does not need to be updated manually unless desired by the user.

#### Creating a PurePursuitCommand

Creating the command is simple. Note that this implementation of the command does not utilize every feature of the Path and is relatively simplistic. It is designed to be a simple template for the user and not an end-all-be-all for every possible desired activity.

To create the object, pass in the drivebase object, the odometry subsystem, and the desired waypoints.

```java
PurePursuitCommand ppCommand = new PurePursuitCommand(
    m_robotDrive, m_odometry,
    p1, p2, p3, p4, p5
);

// schedule the command
ppCommand.schedule();

// remove waypoint at specified index
ppCommand.removeWayPointAtIndex(4);
ppCommand.removeWayPointAtIndex(3);
ppCommand.removeWayPointAtIndex(2);

// add waypoints to the path
ppCommand.addWaypoint(p3);
ppCommand.addWaypoints(p4, p5);
```

The rest of the class does everything for you through the command-based paradigm. Using the `execute()`, `end(interrupted)`, and `isFinished()` methods allows for the command to be run simply by running the scheduler in a loop.

#### Running the Command

It is run the exact same way everything else is run in the paradigm: by running the scheduler. Take a look at [this sample](https://github.com/FTC-23511/SolversLib/blob/master/examples/src/main/java/org/firstinspires/ftc/teamcode/PurePursuitSample.java) to see how everything works together.


# Trajectory

package com.seattlesolvers.solverslib.trajectory

In FTC, there are often games that require an autonomous where robots are moving from one position to another—sometimes repeatedly. A lot of teams implement this motion by moving forward, turning, then moving forward again. Sometimes this is done with a time-base or a unit of known distance.

While these methods are functional, it is better if we can have the robot turn and drive at the same time to optimize the motion. Below is a video showing how this trajectory generation and following works:

{% embed url="<https://youtu.be/yVmJDOE3M2Y>" %}

You can find the same information in the [wpilib docs](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/trajectories/trajectory-generation.html).


# Trajectory Generation

This article goes over how to generate a trajectory. The next few articles in this series will go over how to actually follow the generated trajectory. There are a few things that your robot must have before you dive into the world of trajectories:

* A way to measure the position and velocity of each side of the robot. An encoder is the best way to do this; however, other options may include optical flow sensors, etc.
* A way to measure the angle or angular rate of the robot chassis. A gyroscope is the best way to do this. Although the angular rate can be calculated using encoder velocities, this method is NOT recommended because of wheel scrubbing.

## Splines

A [spline](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/spline) refers to a set of curves that interpolate between points. Think of it as connecting dots, except with curves. SolversLib supports two types of splines: hermite clamped cubic and hermite quintic.

* Hermite clamped cubic: This is the recommended option for most users. Generation of trajectories using these splines involves specifying the (x, y) coordinates of all points, and the headings at the start and end waypoints. The headings at the interior waypoints are automatically determined to ensure continuous curvature (rate of change of the heading) throughout.
* Hermite quintic: This is a more advanced option which requires the user to specify (x, y) coordinates and headings for *all* waypoints. This should be used if you are unhappy with the trajectories that are being generated by the clamped cubic splines or if you want finer control of headings at the interior points.

Splines are used as a tool to generate trajectories; however, the spline itself does not have any information about velocities and accelerations. Therefore, it is not recommended that you use the spline classes directly. In order to generate a smooth path with velocities and accelerations, a *trajectory* must be generated.

## Creating the Trajectory Configuration

A configuration must be created in order to generate a trajectory. The config contains information about special constraints, the max velocity, the max acceleration in addition to the start velocity and end velocity. The config also contains information about whether the trajectory should be reversed (robot travels backward along the waypoints). The `TrajectoryConfig` class should be used to construct a config. The constructor for this class takes two arguments, the max velocity and max acceleration. The other fields (`startVelocity`, `endVelocity`, `reversed`, `constraints`) are defaulted to reasonable values (`0`, `0`, `false`, `{}`) when the object is created. If you wish to modify the values of any of these fields, you can call the following methods:

* `setStartVelocity(double startVelocityMetersPerSecond)`
* `setEndVelocity(double endVelocityMetersPerSecond)`
* `setReversed(boolean reversed)`
* `addConstraint(TrajectoryConstraint constraint)`

### NOTE

The `reversed` property simply represents whether the robot is traveling backward. If you specify four waypoints, a, b, c, and d, the robot will still travel in the same order through the waypoints when the `reversed` flag is set to `true`. This also means that you must account for the direction of the robot when providing the waypoints. For example, if your robot is facing your alliance station wall and travels backwards to some field element, the starting waypoint should have a rotation of 180 degrees.

## Generating the Trajectory

The method used to generate a trajectory is `generateTrajectory(...)`. There are four overloads for this method. Two that use clamped cubic splines and the two others that use quintic splines. For each type of spline, there are two ways to construct a trajectory. The easiest methods are the overloads that accept `Pose2d` objects.

For clamped cubic splines, this method accepts two `Pose2d` objects, one for the starting waypoint and one for the ending waypoint. The method takes in a vector of `Translation2d` objects which represent the interior waypoints. The headings at these interior waypoints are determined automatically to ensure continuous curvature. For quintic splines, the method simply takes in a list of `Pose2d` objects, with each `Pose2d` representing a point and heading on the field.

The more complex overload accepts “control vectors” for splines. The `ControlVector` class consists of two `double` arrays. Each array represents one dimension (x or y), and its elements represent the derivatives at that point. For example, the value at element 0 of the `x` array represents the x coordinate (0th derivative), the value at element 1 represents the 1st derivative in the x dimension and so on.

When using clamped cubic splines, the length of the array must be 2 (0th and 1st derivatives), whereas when using quintic splines, the length of the array should be 3 (0th, 1st, and 2nd derivative). Unless you know exactly what you are doing, the first and simpler method is HIGHLY recommended for manually generating trajectories.

Here is an example of generating a trajectory using clamped cubic splines for the 2018 game, FIRST Power Up:

```java
class ExampleTrajectory {
  public void generateTrajectory() {

    // 2018 cross scale auto waypoints.
    var sideStart = new Pose2d(Units.feetToMeters(1.54), Units.feetToMeters(23.23),
        Rotation2d.fromDegrees(-180));
    var crossScale = new Pose2d(Units.feetToMeters(23.7), Units.feetToMeters(6.8),
        Rotation2d.fromDegrees(-160));

    var interiorWaypoints = new ArrayList<Translation2d>();
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(14.54), Units.feetToMeters(23.23)));
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(21.04), Units.feetToMeters(18.23)));

    TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(12), Units.feetToMeters(12));
    config.setReversed(true);

    var trajectory = TrajectoryGenerator.generateTrajectory(
        sideStart,
        interiorWaypoints,
        crossScale,
        config);
  }
}
```

Note that this is ***not*** code from SolversLib and is instead a sample from WPILib docs. The keyword `var` is used here, but that is a feature from Java 10 which is not supported for Android development. The keyword has the compiler interpret the variable type and assign accordingly. Instead of this, use the actual variable type definition. The Units class is also not available in SolversLib and is instead a WPILib utility.


# Trajectory Constraints

In the [previous article](/0.3.4/pathing/pure-pursuit/trajectory/trajectory-generation), you might have noticed that no custom constraints were added when generating the trajectories. Custom constraints allow users to impose more restrictions on the velocity and acceleration at points along the trajectory based on location and curvature.

For example, a custom constraint can keep the velocity of the trajectory under a certain threshold in a certain region or slow down the robot near turns for stability purposes.

## Provided Constraints

SolversLib includes a set of predefined constraints that users can utilize when generating trajectories. The list of SolversLib-provided constraints is as follows:

* `CentripetalAccelerationConstraint`: Limits the centripetal acceleration of the robot as it traverses along the trajectory. This can help slow down the robot around tight turns.
* `DifferentialDriveKinematicsConstraint`: Limits the velocity of the robot around turns such that no wheel of a differential-drive robot goes over a specified maximum velocity.
* `DifferentialDriveVoltageConstraint`: Limits the acceleration of a differential drive robot such that no commanded voltage goes over a specified maximum.
* `MecanumDriveKinematicsConstraint`: Limits the velocity of the robot around turns such that no wheel of a [mecanum](/0.3.4/features/drivebases#mecanum)-drive robot goes over a specified maximum velocity.
* `SwerveDriveKinematicsConstraint`: Limits the velocity of the robot around turns such that no wheel of a swerve-drive robot goes over a specified maximum velocity.

### Note

The `DifferentialDriveVoltageConstraint` only ensures that theoretical voltage commands do not go over the specified maximum using a [feedforward model](/0.3.4/features/controllers#feedforward-control). If the robot were to deviate from the reference while tracking, the commanded voltage may be higher than the specified maximum.

## Creating a Custom Constraint

Users can create their own constraint by implementing the `TrajectoryConstraint` [interface](https://github.com/FTC-23511/SolversLib/tree/master/core/src/main/java/com/seattlesolvers/solverslib/trajectory/constraint/TrajectoryConstraint.java).

```java
@Override
public double getMaxVelocityMetersPerSecond(
  Pose2d poseMeters,
  double curvatureRadPerMeter,
  double velocityMetersPerSecond) {
  // code here
}

@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
  Pose2d poseMeters,
  double curvatureRadPerMeter,
  double velocityMetersPerSecond) {
  // code here
}
```

The `MaxVelocity` method should return the maximum allowed velocity for the given pose, curvature, and original velocity of the trajectory without any constraints. The `MinMaxAcceleration` method should return the minimum and maximum allowed acceleration for the given pose, curvature, and constrained velocity.


# Manipulating Trajectories

Once a trajectory has been generated, you can retrieve information from it using certain methods. These methods will be useful when writing code to follow these trajectories.

## Getting the Total Duration of the Trajectory

Because all trajectories have timestamps at each point, the amount of time it should take for a robot to traverse the entire trajectory is predetermined. The`getTotalTimeSeconds()`method can be used to determine the time it takes to traverse the trajectory.

```java
// Get the total time of the trajectory in seconds
double duration = trajectory.getTotalTimeSeconds();
```

## Sampling the Trajectory

The trajectory can be sampled at various timesteps to get the pose, velocity, and acceleration at that point. The `sample(double timeSeconds)` method can be used to sample the trajectory at any timestep. The parameter refers to the amount of time passed since 0 seconds (the starting point of the trajectory).

```java
// Sample the trajectory at 1.2 seconds. This represents where the robot
// should be after 1.2 seconds of traversal.
Trajectory.State point = trajectory.sample(1.2);
```

The sample has several pieces of information about the sample point:

* `t`: The time elapsed from the beginning of the trajectory up to the sample point.
* `velocity`: The velocity at the sample point.
* `acceleration`: The acceleration at the sample point.
* `pose`: The pose (x, y, heading) at the sample point.
* `curvature`: The curvature (rate of change of heading with respect to distance along the trajectory) at the sample point.

Note: The angular velocity at the sample point can be calculated by multiplying the velocity by the curvature.


# Transforming Trajectories

Trajectories can be transformed from one coordinate system to another and moved within a coordinate system using the `relativeTo` and the `transformBy` methods. These methods are useful for moving trajectories within space, or redefining an already existing trajectory in another frame of reference.

### Note

Neither of these methods changes the shape of the original trajectory.

## The `relativeTo` Method

The `relativeTo` method is used to redefine an already existing trajectory in another frame of reference. This method takes one argument: a pose, (via a `Pose2d` object) that is defined with respect to the current coordinate system, that represents the origin of the new coordinate system.

For example, a trajectory defined in coordinate system A can be redefined in coordinate system B, whose origin is at (2, 2, 30 degrees) in coordinate system A, using the `relativeTo` method.

```java
Pose2d bOrigin = new Pose2d(2, 2, Rotation2d.fromDegrees(30));
Trajectory bTrajectory = aTrajectory.relativeTo(bOrigin);
```

![](/files/raCpSj7Ja8QrFtxvwrMZ)

In the diagram above, the original trajectory (`aTrajectory` in the code above) has been defined in coordinate system A, represented by the black axes. The red axes, located at (2, 2) and 30° with respect to the original coordinate system, represent coordinate system B. Calling `relativeTo` on `aTrajectory` will redefine all poses in the trajectory to be relative to coordinate system B (red axes).

## The `transformBy` Method

The `transformBy` method can be used to move (i.e. translate and rotate) a trajectory within a coordinate system. This method takes one argument: a transform (via a `Transform2d` object) that maps the current initial position of the trajectory to a desired initial position of the same trajectory.

For example, one may want to transform a trajectory that begins at (2, 2, 30 degrees) to make it begin at (4, 4, 50 degrees) using the `transformBy` method.

```java
Transform2d transform = new Pose2d(4, 4, Rotation2d.fromDegrees(50)).minus(trajectory.getInitialPose());
Trajectory newTrajectory = trajectory.transformBy(transform);
```

![](/files/xfUyvSlK1FFOTOgQVARC)

In the diagram above, the original trajectory, which starts at (2, 2) and at 30° is visible in blue. After applying the transform above, the resultant trajectory’s starting location is changed to (4, 4) at 50°. The resultant trajectory is visible in orange.


# Ramsete Controller

import com.seattlesolvers.solverslib.controller.wpilibcontroller.RamseteController

The Ramsete Controller is a trajectory tracker that is built in to SolversLib. This tracker can be used to accurately track trajectories with correction for minor disturbances.

Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law in addition to the linear ones we have used so far like PID? If we use the original approach with PID controllers for left and right position and velocity states, the controllers only deal with the local pose. If the robot deviates from the path, there is no way for the controllers to correct and the robot may not reach the desired global pose. This is due to multiple endpoints existing for the robot which have the same encoder path arc lengths.

Instead of using wheel path arc lengths (which are in the robot's local coordinate frame), nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this extra information to guide a linear reference tracker like the PID controllers back in by adjusting the references of the PID controllers.

## Constructing the Ramsete Controller Object

The Ramsete controller should be initialized with two gains, namely `b` and `zeta`. Larger values of `b` make convergence more aggressive like a proportional term whereas larger values of `zeta` provide more damping in the response. These controller gains only dictate how the controller will output adjusted velocities. It does NOT affect the actual velocity tracking of the robot. This means that these controller gains are generally robot-agnostic.

## Getting Adjusted Velocities

The Ramsete controller returns “adjusted velocities” so that the when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new goal. The goal comprises of a desired pose, desired linear velocity, and desired angular velocity. Furthermore, the current position of the robot should also be updated periodically. The controller uses these four arguments to return the adjusted linear and angular velocity. Users should command their robot to these linear and angular velocities to achieve optimal trajectory tracking.

The “goal pose” represents the position that the robot should be at at a particular timestep when tracking the trajectory. It does NOT represent the final endpoint of the trajectory.

The controller can be updated using the `calculate` method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter. For the other parameters, one of these overloads takes in the goal as three separate parameters (pose, linear velocity, and angular velocity) whereas the other overload accepts a `Trajectory.State` object, which contains information about the goal pose. For its ease, users should use the latter method when tracking trajectories.

```java
Trajectory.State goal = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
```

These calculations should be performed at every loop iteration, with an updated robot position and goal.

## Using the Adjusted Velocities

The adjusted velocities are of type `ChassisSpeeds`, which contains a `vx` (linear velocity in the forward direction), a `vy` (linear velocity in the sideways direction), and an `omega` (angular velocity around the center of the robot frame). Because the Ramsete controller is a controller for non-holonomic robots (robots which cannot move sideways), the adjusted speeds object has a `vy` of zero.

The returned adjusted speeds can be converted to usable speeds using the kinematics classes for your [drivetrain](/0.3.4/features/drivebases) type. For example, the adjusted velocities can be converted to left and right velocities for a differential drive using a `DifferentialDriveKinematics` object.

```java
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds);
double left = wheelSpeeds.leftMetersPerSecond;
double right = wheelSpeeds.rightMetersPerSecond;
```

Because these new left and right velocities are still speeds and not voltages, two PID controllers, one for each side, may be used to track these velocities.


# Command System

package com.seattlesolvers.solverslib.command

The current command system for SolversLib is modeled closely after that of [WPILib](https://docs.wpilib.org/en/latest/docs/software/commandbased/index.html). Some of the following information may be copied from the source material.

The command-based paradigm is one that allows programming to follow a set design pattern. The specific command system that SolversLib uses follows a declarative programming style. The emphasis is, instead, on what the program *should* do rather than how to do it. This minimizes the iteration-by-iteration robot logic needed to write out a certain action. Very simply, you can bind some actions to buttons/triggers such as the example below:

```java
aButton.whenPressed(intake::run);
```

Or, instead of binding to a button, you can simply schedule an action to occur by registering it.

## Terminology

Let's discuss some of the basic terminology for the paradigm.

### Subsystem

A **subsystem** is the basic unit of robot organization in the design-based paradigm. Subsystems [encapsulate](https://en.wikipedia.org/wiki/Encapsulation_\(computer_programming\)) lower-level robot hardware (such as motor controllers, sensors, and/or pneumatic actuators), and define the interfaces through which that hardware can be accessed by the rest of the robot code. Subsystems allow users to “hide” the internal complexity of their actual hardware from the rest of their code - this both simplifies the rest of the robot code, and allows changes to the internal details of a subsystem without also changing the rest of the robot code. Subsystems implement the `Subsystem` interface.

### Command

A **command** defines high-level robot actions or behaviors that utilize the methods defined by the subsystems. A command is a simple [state machine](https://en.wikipedia.org/wiki/Finite-state_machine) that is either initializing, executing, ending, or idle. Users write code specifying which action should be taken in each state. Simple commands can be composed into “command groups” to accomplish more-complicated tasks. Commands, including command groups, implement the `Command` interface.

#### How Commands Are Run

For a more detailed explanation, please see the [Command Scheduler](/0.3.4/command-base/command-system/command-scheduler).

Commands are run by the `CommandScheduler`, which is a singleton at the base of the command system. The command scheduler is in charge of polling buttons for new commands to schedule, checking the resources required by those commands to avoid conflicts, executing currently-scheduled commands, and removing commands that have finished or been interrupted. The scheduler’s `run()` method may be called from any place in the user’s code.

Multiple commands can run concurrently, as long as they do not require the same resources on the robot. Resource management is handled on a per-subsystem basis: commands may specify which subsystems they interact with, and the scheduler will never schedule more than one command requiring a given subsystem at a time. This ensures that, for example, users will not end up with two different pieces of code attempting to set the same motor controller to different output values. If a new command is scheduled that requires a subsystem that is already in use, it will either interrupt the currently-running command that requires that subsystem (if the command has been scheduled as interruptible), or else it will not be scheduled.

Subsystems also can be associated with “default commands” that will be automatically scheduled when no other command is currently using the subsystem. This is useful for continuous “background” actions such as controlling the robot drive, or keeping an arm held at a setpoint.

When a command is scheduled, its `initialize()` method is called once. Its `execute()` method is then called once per call to `CommandScheduler.getInstance().run()`. A command is unscheduled and has its `end(boolean interrupted)` method called when either its `isFinished()` method returns true, or else it is interrupted (either by another command with which it shares a required subsystem, or by being canceled).

#### Command Groups

It is often desirable to build complex commands from simple pieces. This is achievable by [composing](https://en.wikipedia.org/wiki/Object_composition) commands into “command groups.” A [command group](/0.3.4/command-base/command-system/command-groups) is a command that contains multiple commands within it, which run either in parallel or in sequence. The command-based library provides several types of command groups for teams to use, and users are encouraged to write their own, if desired. As command groups themselves implement the `Command` interface, they are [recursively composable](https://en.wikipedia.org/wiki/Object_composition#Recursive_composition) - one can include command groups *within* other command groups. This provides an extremely powerful way of building complex robot actions with a simple library.


# Subsystems

package com.seattlesolvers.solverslib.command.Subsystem

Subsystems are the basic unit of robot organization in the command-based paradigm. A subsystem is an abstraction for a collection of robot hardware that *operates together as a unit*. Subsystems [encapsulate](https://en.wikipedia.org/wiki/Encapsulation_\(computer_programming\)) this hardware, “hiding” it from the rest of the robot code (e.g. commands) and restricting access to it except through the subsystem’s public methods. Restricting the access in this way provides a single convenient place for code that might otherwise be duplicated in multiple places (such as scaling motor outputs or checking limit switches) if the subsystem internals were exposed. It also allows changes to the specific details of how the subsystem works (the “implementation”) to be isolated from the rest of robot code, making it far easier to make substantial changes if/when the design constraints change.

Subsystems also serve as the backbone of the `CommandScheduler`’s resource management system. Commands may declare resource requirements by specifying which subsystems they interact with; the scheduler will never concurrently schedule more than one command that requires a given subsystem. An attempt to schedule a command that requires a subsystem that is already-in-use will either interrupt the currently-running command (if the command has been scheduled as interruptible), or else be ignored.

Subsystems can be associated with “default commands” that will be automatically scheduled when no other command is currently using the subsystem. This is useful for continuous “background” actions such as controlling the robot drive, or keeping an arm held at a setpoint. Similar functionality can be achieved in the subsystem’s `periodic()` method, which is run once per run of the scheduler; teams should try to be consistent within their codebase about which functionality is achieved through either of these methods. Subsystems are represented in the command-based library by the Subsystem interface.

## Creating a Subsystem

The recommended method to create a subsystem for most users is to subclass the abstract `SubsystemBase` class, as seen in the command-based template:

```java
import com.seattlesolvers.solverslib.command.SubsystemBase;

public class ExampleSubsystem extends SubsystemBase {
  /**
   * Creates a new ExampleSubsystem.
   */
  public ExampleSubsystem() {

  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}
```

This class contains automatically calls the `register()` method in its constructor to register the subsystem with the scheduler (this is necessary for the `periodic()` method to be called when the scheduler runs).

Advanced users seeking more flexibility may simply create a class that implements the `Subsystem` interface.

## Creating a Simple Subsystem

Subsystems are easy to create. They combine different sets of hardware to produce either multiple or one particular task. Hence why they are the basic unit of organization in the command system. Below is a simple example:

```java
package org.firstinspires.ftc.robotcontroller.external.samples.CommandSample;

import com.seattlesolvers.solverslib..command.SubsystemBase;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

/**
 * A gripper mechanism that grabs a stone from the quarry.
 * Centered around the Skystone game for FTC that was done in the 2019
 * to 2020 season.
 */
public class GripperSubsystem extends SubsystemBase {

    private final Servo mechRotation;

    public GripperSubsystem(final HardwareMap hMap, final String name) {
        mechRotation = hMap.get(Servo.class, name);
    }

    /**
     * Grabs a stone.
     */
    public void grab() {
        mechRotation.setPosition(0.76);
    }

    /**
     * Releases a stone.
     */
    public void release() {
        mechRotation.setPosition(0);
    }

}
```

## Setting Default Commands

“Default commands” are commands that run automatically whenever a subsystem is not being used by another command.

Setting a default command for a subsystem is very easy; one simply calls `CommandScheduler.getInstance().setDefaultCommand()`, or, more simply, the `setDefaultCommand()` method of the `Subsystem` interface which can be called in the OpMode:

```java
CommandScheduler.getInstance().setDefaultCommand(exampleSubsystem, exampleCommand);
```

```java
exampleSubsystem.setDefaultCommand(exampleCommand);
```


# Command

package com.seattlesolvers.solverslib.command.Command

Commands are simple state machines that perform high-level robot functions using the methods defined by subsystems. Commands can be either idle, in which they do nothing, or scheduled, in which the scheduler will execute a specific set of the command’s code depending on the state of the command. The `CommandScheduler` recognizes scheduled commands as being in one of three states: initializing, executing, or ending. Commands specify what is done in each of these states through the `initialize()`, `execute()` and `end()` methods. Commands are represented in the command-based library by the `Command` interface.

## Creating Commands

Similarly to subsystems, the recommended method for most users to create a command is to subclass the abstract `CommandBase` class, as seen in the command-based template.

```java
import com.seattlesolvers.solverslib.command.CommandBase;

/**
 * An example command that uses an example subsystem.
 */
public class ExampleCommand extends CommandBase {
  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
  private final ExampleSubsystem m_subsystem;

  /**
   * Creates a new ExampleCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public ExampleCommand(ExampleSubsystem subsystem) {
    m_subsystem = subsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(subsystem);
  }
}
```

As before, this contains several convenience features. It automatically overrides the `getRequirements()` method for users, returning a list of requirements that is empty by default, but can be added to with the `addRequirements()` method.

Also as before, advanced users seeking more flexibility are free to simply create their own class implementing the `Command` interface.

To schedule a command to the scheduler, you will need to call the `schedule()` method of the command instance.

<pre class="language-java"><code class="lang-java"><strong>m_command.schedule();
</strong></code></pre>

To schedule a command as uninterruptible, you can simply schedule it as `false`. The default schedule is `true` (interruptible).

```java
m_command.schedule(false);
```

## The Structure of a Command

While subsystems are fairly freeform, and may generally look like whatever the user wishes them to, commands are quite a bit more constrained. Command code must specify what the command will do in each of its possible states. This is done by overriding the `initialize()`, `execute()`, and `end()` methods. Additionally, a command must be able to tell the scheduler when (if ever) it has finished execution - this is done by overriding the `isFinished()` method. All of these methods are defaulted to reduce clutter in user code: `initialize()`, `execute()`, and `end()` are defaulted to simply do nothing, while `isFinished()` is defaulted to return false (resulting in a command that never ends).

### Initialization

The `initialize()` method is run exactly once per time a command is scheduled, as part of the scheduler’s `schedule()` method. The scheduler’s `run()` method does not need to be called for the `initialize()` method to run. The initialize block should be used to place the command in a known starting state for execution. It is also useful for performing tasks that only need to be performed once per time scheduled, such as setting motors to run at a constant speed.

### Execution

The `execute()` method is called repeatedly while the command is scheduled, whenever the scheduler’s `run()` method is called. The execute block should be used for any task that needs to be done continually while the command is scheduled, such as updating motor outputs to match joystick inputs, or using the output of a control loop.

### Ending

The `end()` method is called once when the command ends, whether it finishes normally (i.e. `isFinished()` returned true) or it was interrupted (either by another command or by being explicitly canceled). The method argument specifies the manner in which the command ended; users can use this to differentiate the behavior of their command end accordingly. The end block should be used to “wrap up” command state in a neat way, such as setting motors back to zero or reverting a solenoid actuator to a “default” state.

### Specifying End Conditions

The `isFinished()` method is called repeatedly while the command is scheduled, whenever the scheduler’s `run()` method is called. As soon as it returns true, the command’s `end()` method is called and it is unscheduled. The `isFinished()` method is called *after* the `execute()` method, so the command *will* execute once on the same iteration that it is unscheduled.

### Simple Command Example

Taking the gripper example from the [Subsystem](/0.3.4/command-base/command-system/subsystems) page, we can develop the following action to grab a stone from the quarry:

```java
import com.seattlesolvers.solverslib.command.CommandBase;

/**
 * A simple command that grabs a stone with the
 * {@link GripperSubsystem}.  Written explicitly for
 * pedagogical purposes. Actual code should inline a
 * command this simple with {@link
 * com.seattlesolvers.solverslib.command.InstantCommand}.
 */
public class GrabStone extends CommandBase {

    // The subsystem the command runs on
    private final GripperSubsystem m_gripperSubsystem;

    public GrabStone(GripperSubsystem subsystem) {
        m_gripperSubsystem = subsystem;
        addRequirements(m_gripperSubsystem);
    }

    @Override
    public void initialize() {
        m_gripperSubsystem.grab();
    }

    @Override
    public boolean isFinished() {
        return true;
    }

}
```

Notice that, here, the gripper subsystem is passed into the constructor in order to produce the command action. This is called [dependency injection](https://en.wikipedia.org/wiki/Dependency_injection), and allows users to avoid declaring their subsystems as global variables. This is widely accepted as a best-practice.

Notice also that the above command calls the subsystem method once from initialize, and then immediately ends (as `isFinished()` simply returns true).

Below is a more complex example of a custom command.

```java
import com.seattlesolvers.solverslib.command.CommandBase;

import java.util.function.DoubleSupplier;

/**
 * A command to drive the robot with joystick input
 * (passed in as {@link DoubleSupplier}s). Written
 * explicitly for pedagogical purposes.
 */
public class DefaultDrive extends CommandBase {

    private final DriveSubsystem m_drive;
    private final DoubleSupplier m_forward;
    private final DoubleSupplier m_rotation;

    /**
     * Creates a new DefaultDrive.
     *
     * @param subsystem The drive subsystem this command will run on.
     * @param forward The control input for driving forwards/backwards
     * @param rotation The control input for turning
     */
    public DefaultDrive(DriveSubsystem subsystem,
        DoubleSupplier forward, DoubleSupplier rotation) {
        m_drive = subsystem;
        m_forward = forward;
        m_rotation = rotation;
        addRequirements(m_drive);
    }

    @Override
    public void execute() {
        m_drive.drive(
            m_forward.getAsDouble(),
            m_rotation.getAsDouble()
        );
    }

}
```

Notice that this command does not override `isFinished()`, and thus will never end.


# Command Groups

package com.seattlesolvers.solverslib.command.CommandGroup

Individual commands are capable of accomplishing a large variety of robot tasks, but the simple three-state format can quickly become cumbersome when more advanced functionality requiring extended sequences of robot tasks or coordination of multiple robot subsystems is required. In order to accomplish this, users are encouraged to use the powerful command group functionality included in the command-based library.

As the name suggests, command groups are combinations of multiple commands. The act of combining multiple objects (such as commands) into a bigger object is known as [composition](https://en.wikipedia.org/wiki/Object_composition). Command groups *compose* multiple commands into a *composite* command. This allows code to be kept much cleaner and simpler, as the individual *component* commands may be written independently of the code that combines them, greatly reducing the amount of complexity at any given step of the process.

Most importantly, however, command groups *are themselves commands* - they implement the `Command` interface. This allows command groups to be [recursively composed](https://en.wikipedia.org/wiki/Object_composition#Recursive_composition) - that is, a command group may contain *other command groups* as components.

## Types of Command Groups

The command-based library supports four basic types of command groups: `SequentialCommandGroup`, `ParallelCommandGroup`, `ParallelRaceGroup`, and `ParallelDeadlineGroup`. Each of these command groups combines multiple commands into a composite command - however, they do so in different ways:

### SequentialCommandGroup

A `SequentialCommandGroup` runs a list of commands in sequence - the first command will be executed, then the second, then the third, and so on until the list finishes. The sequential group finishes after the last command in the sequence finishes. It is therefore usually important to ensure that each command in the sequence does actually finish (if a given command does not finish, the next command will never start!).

### ParallelCommandGroup

A `ParallelCommandGroup` runs a set of commands concurrently - all commands will execute at the same time. The parallel group will end when all commands have finished.

### ParallelRaceGroup

A `ParallelRaceGroup` is much like a `ParallelCommandgroup`, in that it runs a set of commands concurrently. However, the race group ends as soon as any command in the group ends - all other commands are interrupted at that point.

### ParallelDeadlineGroup

A `ParallelDeadlineGroup`also runs a set of commands concurrently. However, the deadline group ends when a *specific* command (the “deadline”) ends, interrupting all other commands in the group that are still running at that point.

## Creating Command Groups

Users have several options for creating command groups. One way - similar to the previous implementation of the command-based library - is to subclass one of the command group classes. Below is an example of a command group:

```java
import com.seattlesolvers.solverslib.command.SequentialCommandGroup;

/**
 * A complex auto command that drives forward,
 * releases a stone, and then drives backward.
 */
public class ReleaseAndBack extends SequentialCommandGroup {

    private static final double INCHES = 3.0;
    private static final double SPEED = 0.5;

    /**
     * Creates a new ReleaseAndBack command group.
     *
     * @param drive The drive subsystem this command will run on
     * @param grip The gripper subsystem this command will run on
     */
    public ReleaseAndBack(DriveSubsystem drive, GripperSubsystem grip)
    {
        addCommands(
                new DriveDistance(INCHES, SPEED, drive),
                new ReleaseStone(grip),
                new DriveDistance(INCHES, SPEED, drive)
        );
        addRequirements(drive, grip);
    }

}
```

The `addCommands()` method adds commands to the group, and is present in all four types of command group.

## Inline Command Groups

Command groups can be used without subclassing at all: one can simply pass in the desired commands through the constructor:

```java
new SequentialCommandGroup(new FooCommand(), new BarCommand());
```

This is called an inline command definition, and is very handy for circumstances where command groups are not likely to be reused, and writing an entire class for them would be wasteful.

## Recursive Composition of Command Groups

As mentioned earlier, command groups are [recursively composable](https://en.wikipedia.org/wiki/Object_composition#Recursive_composition) - since command groups are themselves commands, they may be included as components of other command groups. This is an extremely powerful feature of command groups, and allows users to build very complex robot actions from simple pieces. For example, consider the following code:

```java
new SequentialCommandGroup(
   new DriveToGoal(m_drive),
   new ParallelCommandGroup(
      new RaiseElevator(m_elevator),
      new SetWristPosition(m_wrist)),
   new ScoreTube(m_wrist));
```

Notice how the recursive composition allows the embedding of a parallel control structure within a sequential one. Notice also that this entire, more-complex structure, could be again embedded in another structure. Composition is an extremely powerful tool, and one that users should be sure to use extensively.

## Command Group and Requirements

As command groups are commands, they also must declare their requirements. However, users are not required to specify requirements manually for command groups - requirements are automatically inferred from the commands included. As a rule, *command groups include the union of all of the subsystems required by their component commands.* Thus, the `ReleaseAndBack` shown previously will require both the drive subsystem and the gripper subsystem of the robot.

Additionally, requirements are enforced within all three types of parallel groups - a parallel group may *not* contain multiple commands that require the same subsystem.

Some advanced users may find this overly-restrictive - for said users, the library offers a `ScheduleCommand` class that can be used to independently “branch off” from command groups to provide finer granularity in requirement management.

## Restrictions on Command Group Components

Since command group components are run through their encapsulating command groups, errors could occur if those same command instances were independently scheduled at the same time as the group - the command would be being run from multiple places at once, and thus could end up with inconsistent internal state, causing unexpected and hard-to-diagnose behavior.

For this reason, command instances that have been added to a command group cannot be independently scheduled or added to a second command group. Attempting to do so will throw an exception and crash the user program.

Advanced users who wish to re-use a command instance and are *certain* that it is safe to do so may bypass this restriction with the `clearGroupedCommand()` method in the `CommandGroupBase` class.




---

[Next Page](/llms-full.txt/1)