SwerveIO is an open-source swerve drive library written for FRC in Java. It is pronounced "Swerve - ee - oh", which rhymes with "Oreo". You are currently viewing the official, complete user documentation for this release. Developer documentation is available on the project source page.
SwerveIO is intended to be capable of driving every FRC swerve drive that exists. It features an extremely standard collection of interfaces that all modules, PID controllers, and encoders must adhere to. This makes porting your robot code from swerve drive to swerve drive incredibly simple.
If you need additional help beyond this documentation, please reach out! I would be more than willing to help you and your team. You can always email me at jordan@bancino.net.
SwerveIO aims to be as simple and easy to use as possible, yet also powerful, so that rookies and veterans alike can get the most out of their swerve drive. Everything in this library can be extended and built upon to fit the widest variety of needs. SwerveIO handles all the complex math in an object-oriented way that Java programmers should be familiar with. All you need to do is describe your hardware setup, and SwerveIO will do the rest.
SwerveIO is extremely modular. As you will notice, there are many packages, classes and interfaces that make up the entire SwerveIO API. It is designed to be extensible both in it's current form, and as development progresses. The design goals of SwerveIO are as follows:
Whether you're a first-year student, or 11th-year mentor, SwerveIO is designed to meet your needs. The internals of the API are masked by default so as not to scare rookies off, but can be easily exposed and customized by experienced programmers. The target audience of this documentation is anyone interested in learning how to program an FRC swerve drive with SwerveIO. Ultimately, the math and science behind swerve drive is beyond the scope of this documentation, though the kinematics package does provide a brief overview of why things work the way they do, and links to documents containing the math that is implemented in this library.
This documentation is standard JavaDoc documentation, so please familiarize yourself with JavaDoc before continuing. This documentation assumes you are profficient in the Java programming language, the JavaDoc documentation system, and a build system with which you can comfortably build and deploy Java-based projects. A basic understanding of the git version control system may also be required for some sections of this document. Each component of the SwerveIO API will have thorough comments on the relevant pages of this documentation.
SwerveIO
DefaultSwerveKinematics
or the PID controller (DefaultPIDController
).
Almost every component of SwerveIO can be replaced with custom implementations as well.The main SwerveIO API, called SwerveIO Core, is very lightweight. There are no dependencies beyond the official FRC WPILib project. There is no reliance on any other third-party libraries for implementing any part of SwerveIO; SwerveIO simply supplements the dependencies listed here. As of this release, the complete list of dependencies that SwerveIO declares is as follows:
edu.wpi.first.wpilibj2
package
This release of SwerveIO is intended to be compatible with the following software:
This release of SwerveIO is intended to be compatible with the hardware implementations provided by the official hardware artifacts. All the subpackages of SwerveIO Core provide generic interfaces that you can use to support your own hardware. If support for your hardware is not provided by an official artifact, adding support for it is trivial and can be accomplished by any Java developer using this documentation. One can easily add custom motor controllers, encoders, PID controllers, loggers, and more using the SwerveIO interfaces present and documented in their relevant subpackages.
For a list of kit assemblies that are officially supported by SwerveIO, see the Supported Hardware section. If you don't see your kit listed, supporting it should be easy, but feel free to reach out to us so we can help you.
SwerveDrive
class is the high-level
glue that binds the following lower-level components together:
SwerveIO also provides some additional higher-level features as well:
LogHandler
interface is intuitive and helpful in debugging.
It allows you to monitor the state of the swerve drive over time. You can log to files, the dashboard, and
more.
SwerveIO embraces "functional programming", which allows the programmer to pass a function in as a parameter to a method or constructor. Functional programming is an efficient way program, and it may not be possible to avoid it when working with the SwerveIO API. Please make sure you understand functional programming in Java, and be familiar with lambda expressions, as these greatly simplify the code that needs to be written. Some of the design choices that were made were done so to provide a declarative API with which you simply describe your swerve drive configuration.
This documentation is not intended to be read sequentially, but it would be adventagous to take
a top-down approach to it. Begin with the base package, the commands, and the loggers, all of
which make up the high-level API, then work your way down to the kinematics provider and module
interfaces. It is necessary to have a deep understanding of the way SwerveIO works and how it
is used by the high-level programmer before any attempt to extend hardware support or other
low-level functionality is made. If you are new to SwerveIO, it would be beneficial to start
with the
SwerveDrive.Builder
class documentation, available in the
net.bancino.robotics.swerveio
package. That page will explain what is necessary to create a swerve drive subsystem. Next,
it would be wise to check out the command package, available in
net.bancino.robotics.swerveio.command
,
which contains code for driving the swerve drive with a joystick and autonomously.
If you aren't obtaining SwerveIO from a repository using the standard WPILib vendor library procedure, you can
use SwerveIO in a Gradle composite build. Note that there are no official binary distributions of SwerveIO; it
is only available as source code. If you are using a binary distribution, refer to the distribution
documentation to use SwerveIO with your project. To use SwerveIO in a Gradle composite build, download and
extract the code to a subdirectory of your robot code.. Then add the following line to your
settings.grade
to include the local copy of SwerveIO:
includBuild "SwerveIO-vX.X.X"
Note that X.X.X
is the version of SwerveIO you downloaded. You may want to build your robot code
with the following command to ensure it works properly:
$ gradle build
Note that using a composite build still requires you to have the vendor dependency files installed, or you'll
have to manually add the maven artifacts to your build.gradle
.
If you're at a competition, or just want faster builds, after running the above command to ensure all
dependencies are resolved, you can add the --offline
flag to disable all build features that
require the internet.
This documentation assumes that you have successfully configured SwerveIO for use in your robot code by adding it to your build system of choice. SwerveIO is a Gradle project, which uses the Maven repository model. FRC switched from Apache Ant to Gradle a few years ago. Whatever your build system, it is assumed that you have configured it to allow the use of SwerveIO. This documentation only details the SwerveIO API. Anything upon which the understanding and usage of this API depends is considered a prerequisite that is to be fulfilled by the reader.
Starting with SwerveIO 6.0.0, the entire SwerveIO API is broken up into three types of components:
SwerveDrive
subsystem, and commands that drive the SwerveIO drive system.
Each component is maintained as a Gradle subproject in the SwerveIO repository and may be published as a
separate Maven artifact. Below is an exhaustive list of all the officially supported hardware. You can add this
hardware support to your code by dropping the vendor JSON files generated by the publish
Gradle
task into your vendordeps
folder.
Vendor | Hardware |
---|---|
Cross The Road Electronics | All motor-attached encoders, and the CANCoder |
RevRobotics | Spark Max-attached encoders |
Kauai Labs | NavX Gyro |
Vendor | Kits |
---|---|
Swerve Drive Specialties | MK2, MK3 |
These kits will automatically pull in the required vendor dependencies, so you don't have to include hardware libraries explicitly unless you're using them outside of the kit implementation.
If you want to mock up a swerve drive configuration for unit testing or other experimental purposes, you can
pull in the SwerveIO-Virtual
project, which provides a swerve module implementation that is not
tied to any hardware at all.
There are a number of different terms that this library uses. It is important to understand them to understand how this library works. The terms used throughout this documentation and the SwerveIO code are listed below, and defined how they are used in this library.
SwerveVector
, direction is indicated with the sign of the values, but ModuleVector
has an angle
field that represents the direction. It all depends on the use of the vector. All
vectors also have very specific bounds, so please read the documentation on them before using them.
This appendix provides small help documents to get you started working with SwerveIO. Everything covered in this appendix is available in the standard Java documentation, but these entries are designed to provide "tips and tricks" for using SwerveIO. Things that may not be immediately obvious from reading the documentation but are common enough tasks are documented here.
Adding new hardware support for SwerveIO is quite simple using GenericSwerveModule
.
GenericSwerveModule
allows you to declare the hardware properties of your swerve module.
The point of adding hardware-specific classes to SwerveIO is to provide a convenient place for tuning parameters
and direct hardware configuration. End users should be able to simply instantiate a new module object, say
MK3SwerveModule
without caring what the gear ratios or PID parameters are. Hardware-specific swerve
module classes make this possible by hard-coding in all the required properties such as gear ratios, motor RPMS,
and wheel diameters. It is also highly recommended to provide sensible default PID tuning parameters. Any
module-specific initialization that needs to occur (such as reversing the direction of a motor) should also
happen here so that users can simply instantiate the module and start running.
SwerveIO provides "official" support for multiple swerve modules, which means that all the numbers have been crunched and hard-coded for you if you choose to use those modules. If you choose not to use "official" modules, you'll have to write your own class for them. This appendix entry aims to assist you in this task, which is easy from a programming perspective, but can be difficult because there are lots of numbers that need to be calculated or discovered by experimentation.
At the very least, you'll need the following measurements:
You also need to consider your encoder source. If you are not using an external 1:1 encoder for the pivot motor, you must also provide a pivot gear ratio. This most likely isn't documented well if you're using a kit, because kit builders often push you to use the 1:1 encoder, so you may need to verify this ratio experimentally.
If you're using an integrated encoder, you may need to reverse the direction of the pivot motor by calling
setInverted()
on it. If you get your swerve drive running but the wheels don't go to the correct
angles, try this before changing anything else.
Regardless of what encoder you're using, be it external or integrated, it has to implement the
Encoder
interface. The Encoder
interface is a simple wrapper for hardware
vendor-specific encoder reading classes that enables any encoder to be used with SwerveIO. See the Encoder documentation for the
specifications. SwerveIO already has support for multiple encoders, so check the encoder package before
implementing the interface for yourself.
Once you have all the required information, as well as an Encoder
class, create a new class that
extends GenericSwerveModule
. Consider accepting the following in your constructor:
These things will differ from robot to robot and even module to module, so you don't want to make any assumptions on them. You may also consider accepting a pivot encoder in your constructor as well if you aren't using the encoder integrated into the pivot motor (if equipped). If you want to support both CAN and PWM, you may also provide constructors for these, or add an enumeration to allow users to specify the mode of operation.
The next step is to call the super constructor with an annonymous builder object. This is precisely what the "offical" modules do. Consider the following example:
import net.bancino.robotics.swerveio.module.GenericSwerveModule; import net.bancino.robotics.swerveio.geometry.Length; import net.bancino.robotics.swerveio.geometry.Length.Unit; public class MySwerveModule extends GenericSwerveModule { public MySwerveModule(int driveCanId, int pivotCanId) { super(new GenericSwerveModule.Builder() .setDriveMotor(/* Create your drive motor here */, (builder, motor) -> { /* Configure/initialize the drive motor in this block */ builder.setDriveEncoder(/* Create your drive encoder here */); }) .setPivotMotor(/* Create your pivot motor here */, (builder, motor) -> { /* Configure/initialize the pivot motor in this block */ builder.setPivotEncoder(/* Create your pivot encoder here */); }) /* Declare the module specifications */ .setDriveGearRatio(/* Set the drive gear ratio here */) .setPivotGearRatio(/* Set the pivot gear ratio here */) /* Only required if encoder isn't 1:1 */ .setWheelDiameter(/* Set the wheel diameter here */) .setMaxDriveRPM(/* Set the drive motor's max drive RPMs here */) ); } }
It is highly recommended to follow the constructor conventions of the modules implemented in this class, so you don't confuse other potential users. If you did everything properly, that is a complete swerve module implementation. As you can see, it declaratively configures and initializes the hardware. SwerveIO will take care of the rest. You should now be able to add instances of your swerve module implementation to the SwerveIO module map. See Show Me Code and SwerveDrive.Builder for more details.
Generating the swerve module angle offsets necessary for the pivot functionality can be the most difficult part of setting up your swerve drive. This appendix entry provides detailed instructions for successfully completing this task with SwerveIO. These instructions only apply for absolute encoders. If you use relative encoders, you will have to align your swerve drive each time you power on your robot.
The first step is to ensure that your swerve modules are placed in their forward position. This means making sure all the wheels are aligned and pointed to the front of your robot. You can use a custom alignment frame, or a couple pieces of long wood, metal, or pipe. Team 6090 uses thin blocks of metal that are long enough to reach across both modules on one side. We first rotate the modules by hand until they're generally in the forward position, then we press the long metal into both wheels, ensuring that they are even with each other. We then repeat on the other side of the swerve chassis.
As of SwerveIO 6.1.0, you no longer need to manually calculate the angle offsets. At this point, once you're
aligned, you can call saveAngleOffsets(), either on
code initialization or via a command—see the SaveSwerveAngleOffsets
command. It's up to you
how you want to do this. Team 6090 recommends mapping a
dashboard button for this purpose, and this functionality is provided in the SaveSwerveAngleOffsets
command. After the initial save, you can instruct SwerveIO to initialize with these
offsets using loadAngleOffsets().
If you wish to manually set the offsets, you need some kind of feedback to the driver station. See Shuffleboard for instructions on using Shuffleboard to get information on your swerve drive.
In your robot code, ensure that no existing offsets are applied. This means setting all your module offsets to zero. This ensures that you are getting an accurate angle reading. If all your modules are aligned, write down the current angle readings for each module. Be as precise as possible. These angles are arbitrary because they are calculated from the encoder, which is most likely not "zero" at the forward position, which is why this angle offset is necessary The current reported angle of the swerve module when it is aligned is the offset for that module. You can provide these numbers wherever an "angle offset" value is required.
You don't have to perform this process every time you use your robot; absolute encoders store their position across power losses, so your swerve drive will remember these angles. The offset basically just tells SwerveIO the encoder reading at which each swerve module is in it's forward position. You will have to repeat this process every time you disassemble the swerve module, because the motor, motor shaft, gears, or wheel alignment may be modified during assembly or repairs. Team 6090 also "re-calibrates" the swerve drive before every competition to ensure accuracy.
Wherever possible, SwerveIO provides a unified interface for all vendor-specific classes and methods. This method is not flawless, because often features don't line up exactly, which can result in some PID controller methods throwing an exception if a particular feature isn't supported by the underlying vendor library. On the flip side, sometimes vendors offer functionality not explicitly required for SwerveIO's operation, which means these features are inaccessible through the SwerveIO interfaces. Luckily, it is not that difficult to access these vendor-specific features through casting. This requires knowledge of the underlying vendor libraries and a particular swerve module implementation. If needed, please take a look at the source code so you can understand how the following example works in order to apply it to your own situation.
Consider that you are using MK3SwerveModules
and you are running a velocity loop on the drive
motors. You can set PIDF values using SwerveIO's PhoenixPIDController
, but that only provides basic
configuration. The CTRE Phoenix library provides a large amount of configuration options that SwerveIO can't
possibly hope to provide support for. To access these options, familarize yourself with
SwerveDrive.Builder
's setModuleMap()
methods. One implementation of setModuleMap()
takes a module initializer, which can be used to
perform additional initialization on a swerve module before passing it to the swerve drive controller. It is
through this method that you can access all the methods of the SwerveModule
interface, and thus
access vendor-specific methods as well.
Here's an example snippet. Refer to Show Me Code to see where this fits into your swerve drive setup code.
/* swerve is a reference to a SwerveDrive */ swerve.forEachModule((location, module) -> { /* The code in this block is run on all the swerve modules in the module map. */ /* Use SwerveIO interface methods to set tuning parameters. This will run on any module. */ PIDController pid = module.getPivotPIDController(); pid.setP(0.1); pid.setI(0.001); pid.setD(0); /* Use vendor-specific method via casting. This will only run on modules that use TalonFX controllers. */ WPI_TalonFX driveMotor = (WPI_TalonFX) module.getDriveMotor(); driveMotor.clearStickyFaults(); driveMotor.setIntegralAccumulator(0.001); });
We are most interested in the last few lines. Here, you can see that we cast the swerve module's drive motor to
a WPI_TalonFX
(remember that we're using an MK3SwerveModule
, which uses those motor
controllers) to access additional configuration parameters. This is the easiest way to set custom configuration
parameters on vendor-specific hardware for every swerve module. This method depends on you knowing the
underlying motor controller implementation being used by a swerve module. This information is all documented in
the respective documentation.
It's up to you to read the relevant documentation for getting SwerveIO up and running, but if you're really
struggling putting the pieces together, here's an example that uses SwerveIO with MK3SwerveModule
.
You should get the general idea, and indeed you should be able to copy-paste this example, making only minor
adjustments, but it is not recommended you do so because you won't learn how SwerveIO works. This listing is for
reference only, and even though you may use a different setup, you'll find this to be a good template. Also
remember that this is not a complete reference; it does not showcase all SwerveIO features in the slightest. For
that you'll have to read the documentation.
package frc.robot.subsystems; import net.bancino.robotics.swerveio.SwerveDrive; import net.bancino.robotics.swerveio.module.SwerveModule; import net.bancino.robotics.swerveio.module.MK3SwerveModule; import net.bancino.robotics.swerveio.gyro.Gyro; import net.bancino.robotics.swerveio.geometry.Length; import net.bancino.robotics.swerveio.geometry.Length.Unit; import net.bancino.robotics.swerveio.geometry.ChassisDimension; import net.bancino.robotics.swerveio.pid.PIDController; import net.bancino.robotics.swerveio.log.DashboardLog; /** * The drivetrain subsystem drives the robot. Call create() to * get a new instance of a SwerveIO drivetrain subsystem. * * This subsystem consists of the following components: * - Swerve module (4x drive + pivot motor) * * This subsystem should provide an interface for the * following functions: * * - Running the drivetrain with joystick * - Running the drivetrain autonomously * * @author Jordan Bancino */ public class DriveTrain { /* Your robot chassis dimensions - measured from wheel to wheel. */ private static final ChassisDimension CHASSIS_DIMENSIONS = new ChassisDimension(new Length(22.5, Unit.INCHES)); /* CAN IDs - Set these according to your configuration in Phoenix Tuner */ private static final int FRONT_RIGHT_DRIVE_MOTOR = 5; /* Module 1 */ private static final int FRONT_LEFT_DRIVE_MOTOR = 6; /* Module 2 */ private static final int REAR_LEFT_DRIVE_MOTOR = 7; /* Module 3 */ private static final int REAR_RIGHT_DRIVE_MOTOR = 8; /* Module 4 */ private static final int FRONT_RIGHT_PIVOT_MOTOR = 1; /* Module 1 */ private static final int FRONT_LEFT_PIVOT_MOTOR = 2; /* Module 2 */ private static final int REAR_LEFT_PIVOT_MOTOR = 3; /* Module 3 */ private static final int REAR_RIGHT_PIVOT_MOTOR = 4; /* Module 4 */ private static final int FRONT_RIGHT_ENCODER = 9; /* Module 1 */ private static final int FRONT_LEFT_ENCODER = 10; /* Module 2 */ private static final int REAR_LEFT_ENCODER = 11; /* Module 3 */ private static final int REAR_RIGHT_ENCODER = 12; /* Module 4 */ /* Angle offsets in degrees - See SwerveIO appendix for setting these. */ public static final double FRONT_RIGHT_ANGLE_OFFSET = 146.27; public static final double FRONT_LEFT_ANGLE_OFFSET = 134.60; public static final double REAR_LEFT_ANGLE_OFFSET = 59.34; public static final double REAR_RIGHT_ANGLE_OFFSET = 267.2; public static SwerveDrive create(Gyro gyro) { return new SwerveDrive.Builder() .useDefaultKinematics(CHASSIS_DIMENSIONS) .setGyro(gyro) .setModuleMap((map) -> { map.put(SwerveModule.Location.FRONT_RIGHT, new MK3SwerveModule( FRONT_RIGHT_DRIVE_MOTOR, FRONT_RIGHT_PIVOT_MOTOR, FRONT_RIGHT_ENCODER, FRONT_RIGHT_ANGLE_OFFSET )); map.put(SwerveModule.Location.FRONT_LEFT, new MK3SwerveModule( FRONT_LEFT_DRIVE_MOTOR, FRONT_LEFT_PIVOT_MOTOR, FRONT_LEFT_ENCODER, FRONT_LEFT_ANGLE_OFFSET )); map.put(SwerveModule.Location.REAR_LEFT, new MK3SwerveModule( REAR_LEFT_DRIVE_MOTOR, REAR_LEFT_PIVOT_MOTOR, REAR_LEFT_ENCODER, REAR_LEFT_ANGLE_OFFSET )); map.put(SwerveModule.Location.REAR_RIGHT, new MK3SwerveModule( REAR_RIGHT_DRIVE_MOTOR, REAR_RIGHT_PIVOT_MOTOR, REAR_RIGHT_ENCODER, REAR_RIGHT_ANGLE_OFFSET )); }) .build((swerve) -> { swerve.getLogger().outputTo(new DashboardLog()); }); } }
If you're looking for more examples, check out Team 6090's Github
Organization. There we have a SwerveIOTestBase
and our InfiniteRecharge
code,
both of which utilize SwerveIO. It is up to you to integrate SwerveIO into your robot code; that is beyond the
scope of this documentation. We find that this static create()
method works the best for us because
it isolates our swerve drive configuration in its own file or method, but you may use SwerveIO however works
best for you. SwerveIO is designed to be usable in a wide variety of configurations. While it was designed to be
command-based, there is certainty nothing stopping anyone from using it in other ways.
You can have SwerveIO automatically output a plethora of information on your swerve drive using the
DashboardLog
. This class writes useful information—including encoder positions and module
angles—to the Smart Dashboard. To enable this feature on your swerve drive, use the following code:
import net.bancino.robotics.swerveio.SwerveDrive; import net.bancino.robotics.swerveio.log.DashboardLog; /* ... */ SwerveDrive swerve = /* Fetch or create your swerve drive here. */; swerve.getLogger().outputTo(new DashboardLog());
You simply call outputTo()
on your swerve drive's logger with a new DashboardLog
object. When your robot code runs, you should see live feedback from your swerve drive under the "SwerveIO"
section of your dashboard.
Copyright (C) 2019-2021 Jordan Bancino <jordan@bancino.net>
All parts of SwerveIO, including the source code, build scripts, and documentation, are licensed under the GNU GPL V3. Please read it carefully before using SwerveIO for any purpose. While you do have the right to use and modify SwerveIO for most purposes, you absolutely cannot use SwerveIO in proprietary software. Any software that uses SwerveIO or a derivative of SwerveIO MUST be open source in the sense that anyone can easily obtain the source code that uses SwerveIO. Furthermore, any derivative of SwerveIO must also be open source and bound by the GNU GPL V3. This is to foster an open development environment. SwerveIO is graciously released free of charge to the community, so please adhere to the terms of the GNU GPL V3, and before forking SwerveIO, consider contributing to the upstream project. I want SwerveIO to be the best that it can be for everyone, so if you have an improvement to share, it would be greatly appreciated if you offered it to the main project instead of making the changes privately in your own fork.
You must not remove any copyright notices that appear in this code, either in comments or in console output.