Note

To find the MOST updated versions of any 2875 code linked here, you may need to head into the "develop" branch of the respective repository. However, maintain caution as this code may not be completely functional, and, thus, I would GENERALLY suggest that you stick to the "main" branch.

Overview

In this post, I will go over the most complex MANDATORY component of swerve drive. Although I will be referring to this part as the "drive train", it is beneficial to think of it as a "conductor" for the entire robot; the "conductor" will take in input directly from the user (2875 uses a dual joystick HOSAS control scheme), convert the inputs to a "field-relative" version, split the inputs up into individual commands for each module. This installment in the tutorial will go over the bare minimum for a field oriented swerve drive "conductor". In future installments, I will continue to develop the swerve drive train by adding more features such as angular drift correction and positional drift correction.

Requirements

So far throughout this tutorial series, the math has been relatively light and simple. However, now, you will need (at least) a basic understanding of vectors. The introduction to vector mathematics given in most AP/Honors level high school physics courses should generally be sufficient; however, if you feel a little unsure, now is a great time to brush up on your knowledge (MathIsFun, Wikipedia). The most important concepts are vector addition, and an understanding of the differences between polar vectors and Cartesian vectors.

A Brief Note on WPILib

At this point in the tutorial, you may again be wondering "Why not simply use the WPILib swerve utilities?". The answer to this question, is that you certainly can; however, if you do you will lose many of the benefits of creating your own custom swerve drive. Even if you use the WPILib utilities, I still recommend going through this tutorial so that you can gain a true understanding of their workings under the hood. What I have been referring to as the "drive train" or "conductor" is roughly equivalent to the SwerveDriveKinematics class found in WPILib. After this, you should have enough of an understanding of swerve drive to build an autonomous, and the various other QOL additions on your own, but I will still continue with the tutorial series in order to make it easier. Regardless of if you follow this tutorial series to the "t" or branch off significantly, the better understanding you will gain into swerve drive will more than pay for itself in terms of features you will be able to add over the WPILib utilities. For a brief example, 2875's swerve autonomous contains a few features that the WPILib autonomous swerve utilities do not: an autonomous that is both PID corrected and motion profiled, as well as PID based teleop drift correction through the use of odometry and a gyroscope. While it is possible to add these features using the WPILib, it will be more difficult and may involve the modification of their libraries.

Utility

The train is the most math-involved piece of swerve drive, and thus it will have the most utility methods.

Math

To begin, we will create some methods to handle conversions between the two coordinate systems we will be using: polar and Cartesian.

public double[] polarToCartesian(double theta, double r) {
    //convert a polar coordinate (angle (theta) and magnitude (r) to cartestian (x, y))
    double x = r * Math.cos(Math.toRadians(theta));
    double y = r * Math.sin(Math.toRadians(theta));
    return new double[] { x, y };
}

This method performs some basic trigonometry to convert from a polar coordinate to a Cartesian coordinate. If you are using Java and trigonometry, make sure to remember to convert degrees to radians and vice versa, otherwise you may end up with many hours of annoyance (I sure did) when you forget.

Our next method will do the opposite, and this will be slightly more complex.

public double[] cartesianToPolar(double x, double y) {
    double r = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2));
    double theta = Math.toDegrees(Math.atan2(y, x));

    return new double[] { theta, r };
}

I recommend taking some time to go over these methods and make sure you fully understand the underlying concepts and math, as they will be used repeatedly throughout swerve. The Mathatan2() is of particular importance; you may think it is a simple tangent function, however, in reality, it does much more. The function solves the issue that arises with the existence of multiple quadrants in the coordinate plane, and the issue that comes with converting an (x, y) coordinate to an angle relative to the positive x-axis. If you take a few minutes to think about this problem, you will realize that there are several situations wherein you must convert the coordinate differently, and this is dependent on its quadrant. The Math.atan2() function saves you some time, and code.

Optionally, you can make a class to encapsulate these "angle" types, as well as vectors. This can make your code appear cleaner, and I do recommend it. You can also optionally use SWOLib and the utility classes found there.

In this tutorial, sticking with our theme of allowing the reader to dictate the majority of the code, we will only show an example implementation of a simple method to add vectors.

public double[] addVectors(double[] v1, double[] v2) {
    return new double[] {v1[0] + v2[0], v1[1] + v2[1]};
}

Gyroscope

Team 2875 found it of great help to create a (static) wrapper class to encapsulate our NavX gyro, as it made our drive train code a bit cleaner. While technically optional, I strongly suggest you do this as well. Since the wrapper class will not contain any complex logic and will simply provide a unified way to use the NavX gyro, I will not go over every detail in the tutorial. As usual, you can find a (slightly) outdated Java version of 2875's gyro wrapper class here or an updated Kotlin version here

The only aspect of the gyroscope that I will go over in this tutorial pertains to the robot's field orientation: its offset. In the 2875 Gyro class, you may find the setOffset method to be confusing. This method will "reset" a constant offset on the gyro that allows us to dictate the direction considered "0" degrees. This allows us, and the driver, to redefine the field-orientation of the swerve drive on the fly, and to counter gyro drift. If you need a reminder as to the significance of "field orientation", it refers to a form of holonomic (swerve is a subset of holonomic drive) train in which "forward" (in terms of input) always remains the same direction. This means that no matter how much the robot spins pushing forward on the joystick will always result in movement in the same direction. You will be able to see how this is achieved later on in this post; for now, just now that the "offset" allows the "forward" direction to be set/changed.

2875 binds this method to a button on our control sticks, and recommends that our drivers press it and "reset" the gyro (in reality only updating the offset variable and changing the direction considered 0) whenever they notice drift, or, ideally, after several 360 degree turns of the robot (especially if the turns are performed at high speed). Team 2875 found the drift on the gyroscope to be easily manageable at only a few degrees per full turn.

Code Setup

You should create a class to encapsulate the "conductor", or drive train as a whole, as it is a very complex piece of swerve drive and only grows in complexity as parts are added. At a minimum, this class must contain as many instances of the SwerveModule class as exists on your robot. For a four wheeled swerve drive train, you must have 4 instances of this class. For the purpose of this tutorial, although alternate swerve drive configurations exist, we will assume a standard four wheel swerve configuration.

As previously mentioned, I suggest the creation of a static "Constants" file for use throughout your codebase. This file will be used to store any sort of guaranteed physical "constant" such as motor ports, conversion values, or set positions.

First, we will define our modules/wheels (you can either put them in an array or define them individually, it comes down to personal preference).

    //pass in the turn/drive motor and encoder ports
    SwerveModule backLeftModule = new SwerveModule(Constants.backLeftTurnMotorPort, Constants.backLeftDriveMotorPort, Constants.backLeftEncoderPort);
    SwerveModule backRightModule = ...
    SwerveModule frontLeftModule = ...
    SwerveModule frontRightModule = ...

I personally combine the usage of individually declared modules and an array containing said modules for ease-of-use, which should be defined like so (if you choose):

    public SwerveWheel[] wheelArr = new SwerveWheel[4] {backLeftModule, backRightModule, frontLeftModule, frontRightModule};

You should also (in your constants file) define some set angles for your modules to twist (this will be used in the swerve calculations and allows fine-tuning of each individual module to account for any sort of physical/mechanical issues). As a basis, you should use the following angles (I suggest only performing minimal changes).

    public static final HashMap<String, Integer> twistAngleMap = new HashMap<>() {
        {
            put("frontRight", 45);
            put("frontLeft", 135);
            put("backRight", -45);
            put("backLeft", -135);
            }
    };

You should notice that these angles are all somewhat "opposite" to each other and position the robot's wheels to form a rectangular shape offset from the main chassis.

You may also want to define another hash map for twist speed multipliers for each module (I found that with 2875's tuning and robot scaling the twisting speed down by about 50% made the robot significantly more controllable).

    public static final HashMap<String, Double> twistSpeedMap = new HashMap<>() {
            {
                    put("frontRight", .6);
                    put("frontLeft", .6);
                    put("backRight", .6);
                    put("backLeft", .6);
            }
    };

Theory/Math

This is where the fun begins!

As mentioned in the introductory post in this series, swerve drive functions on a basis of somewhat simple vector math, something that should be doable for anyone who has taken a physics course (or related math course), especially the AP level physics courses. To put it as simply as possible, we must take in two inputs (from the joystick(s)) and split it 4 ways. There are several ways to accomplish this, and the math can be condensed into a few short lines; however, 2875 find it more clear to split the math into several small functions, and while this may not be necessary for people with a concrete understanding of vector math, it is incredibly helpful for those who don't.

To get into the nitty-gritty of the math, we will split this process up into 4 steps. First, for each individual module, we must take the input (from the joystick) and convert the pure (x, y) input vector from Cartesian form to polar form. This is technically only necessary for field orientation; however, since swerve doesn't fully function without field orientation, I consider it a requirement. We do this in order to be able to add the robot's current "angle/direction" reported by the gyro to the input. Once we have the pure (x, y) joystick input in polar form we can, quite literally, add this gyroscope angle to the "theta" value of this polar coordinate. For now, we will leave this value alone.

Next, you must convert the "twisting" input from polar to Cartesian. You may be wondering, "how is the twist input a full polar vector? Doesn't it only have magnitude from the joystick?" and yes, you are correct, the joystick can only provide the magnitude for twisting. This is where our constant twist angle hash map from earlier will come into play. We must take the values from the map for each module and treat that as our theta/angle in the twist polar vector. At this point, you can also (optionally) multiply the joystick input (magnitude) by the constant twist speed multiplier from our other hash map. We will then take this polar vector and (you guessed it) convert it to Cartesian form.

Now, we have two separate Cartesian vectors, and what does that mean? Why it's time to add them, of course! So, we can simply add the "exes" and "whys" of each vector together. Now, we have our combined Cartesian vector for each individual wheel that accounts for both twist and strafe/translation, yay! Now, I swear, for the last time, we must finally convert this Cartesian vector back to a polar vector. This will allow us to directly feed a desired speed and angle to each module's drive function.

At this point, you may also (optionally) "normalize" each individual module's desired speeds around a minimum and maximum speed. This can be done to prevent certain scaling issues arising from the addition/manipulation of the input vectors. 2875 did not find this to be necessary; however, it doesn't hurt to add. You can use any sort of normalization function and simply pass in all the module speeds, a minimum speed, and a maximum speed.

The Actual Code (drive function)

Again, as previously mentioned in this series, complete code for a swerve robot can be found on 2875's GitHub organization, both in Kotlin (updated) and Java (outdated). However, both of these swerve drive trains are more advanced than the one described in this blog post (they build upon it with various other additions that will be described at later points in this series). Here, I will try to go over a few important (and potentially confusing) components.

First, the prototype for your drive funtion.

    public void drive(double inputX, double inputY, double inputTwist)

The function should simply take in 3 axis of joystick inputs for X translation, Y translation, and twisting.

We will then check if the inputs are all 0 (withing the function), and if so, prevent any movement in the modules. This is done to prevent the module reverting to a desired angle of 0 but allowing it to return to a desired speed of 0 upon stopping. This also lessens wear and tear on the hardware.

    if (inputX == 0 && inputY == 0 && inputTwist == 0) {
        backRight.preserveAngle();
        backLeft.preserveAngle();
        frontRight.preserveAngle();
        frontLeft.preserveAngle();
        return;
    }

"Why can't you just return out of the drive function?" You could, but then you would be unable to ensure each module remains at its (respective) previous angle without any speed.

Now, we are going to take a break form the drive function in order to create a few other functions to perform the swerve map (you can compress it all down into the drive function, but the code will be cleaner if it is split up a bit).

We will start off with the "base" function for the swerve drive math: this function will be the one called form the drive function.

    public double[] calculateDrive(double x1, double y1, double theta2, double r2, double twistMult) {
            // Gets the cartesian vector of the robot's joystick translation inputs
            double[] driveVector = fieldOriented(x1, y1, Gyro.getAngle());
            // Turns the twist constant + joystick twist input into a cartesian coordinates
            double[] twistVector = polarToCartesian(theta2, r2 * twistMult);

            // Vector math adds the translation and twisting cartesian coordinates before turning them into polar and returning their (polar) result
            return cartesianToPolar( driveVector[0] + twistVector[0], driveVector[1] + twistVector[1]);
    }

Next, we will write the fieldOriented function. This function is quite simply and merely converts converts its (x, y) joystick inputs to a polar coordinate so the gyro-reported angle can be taken into account (to ensure the same direction is always forwards)

    public double[] fieldOriented(double x, double y, double gyroAngle) {
        // turns the translation input into a polar vector
        double[] polar = cartesianToPolar(x, y);
        // adds the gyro angle to the angle of the translation of the robot (in polar form)
        // this makes it field oriented
        double theta = polar[0] + gyroAngle;

        double r = polar[1];

        // returns the new field oriented translation but converted to a cartesian vector (so it can eventually be added to the twist cartesian vector at the end of the `calculateDrive` function)
        return polarToCartesian(theta, r);
    }

Now, you should go back to the drive function, call the calculateDrive with each module's respective values, and pass the output to each module in order to control the angle and speed of the wheels. As a light exercise (to gain some familitary with the drive train), you can finish off the drive function. Again, all that is remaining is to actually call the calculateDrive function for each module (with their constants) and pass it into the module's individual drive functions. Also, optionally, at this point you can add in the normalization (outlined in the #Future section).

Summary

This was a BIG blog post, and, in the future, I will try to split anything this size (or larger) into multiple sub-pieces so they can be released sooner. At this point, you should have all the skills necessary to program a working (albeit rudimentary) swerve drive robot. Congratulations! now, you can take some time to play around with your robot, and put it through some stress-tests to make sure there are no issues. However, unfortunately (or luckily, depending on how you look at it), you are far from done with programming swerve drive. You still need to add in some autonomous code, and while it is possible (and simple) to hard-code in some movements, I recommend you take the time to build a dynamic, motion-profiled, PID-corrected, position-tracking autonomous mode. This will allow your robot to stand out, increase its performance during autonomous, and (drastically) increase your QOL once it is complete (it will be vastly simpler to create new configurations for autonomous). In future tutorials, I will go over how to accomplish this, so stay tuned.

Future

At this point, you have (mostly) the basis necessary to program the rest of swerve drive, including autonomous/odometry. However, I will still go over those at later points in the tutorial. Here, even if you choose to continue to follow this tutorial series, there are a plethora of possible expansion. Here is a (non-definitive) list:

1. Throttle - In reality this is more of a "speed multiplier", but it functions similarly to a throttle. This expansion is very simple and possibly the most useful (for your drivers). The addition of a simple throttle controlled by your joystick (2875 bound several control methods on our T.16000M joysticks: actual throttles on the bottom, buttons on the top (presets), and the mini-joycons at the top (small increments)) can make a night and day difference while driving (I also drive swerve in competition). 2875 uses a simple multiplier on the input x, y, and twist.
2. Disable Field Orientation - While I did say that field orientation is crucial to swerve drive, there is a single scenario in which it can be beneficial to disable it. When driving via camera (without being able to see the robot/the field) it is pretty much impossible to use field orientation, so having a physical toggle that can disable it will be incredibly helpful in competition. 2875 also has limelight ball tracking (this will be covered in a later post) and we discovered that it is much easier to have the robot "auto pick-up" balls/objects by disabling field orientation and essentially driving straight forward while twisting via limelight. This is also very simple to implement and comes down to a boolean being passed through your code.
3. Wheel Speed Normalization - If you wish, you can "normalize" the speed inputs (the magnitude of the polar vectors) for each of your modules (after the inputs are calculated in the calculateDrive function). This will allow you to ensure that in the off-chance the vector math causes the values to increase (or decrease) disproportionally to the joystick inputs, you can "normalize" them around your desired minimum and maximum values. You can use a general "normalization" formula and examples can be found on the 2875 Cyberhawks GitHub in both Kotlin and Java, altough I recommend looking at our Kotlin code as the Java normalization code contained several bugs.
4. X,Y Drift Correction - Unfortunately, due to the mechanical complexity of swerve drive, it is entirely possible that you will end up with some sort of "drift" or slight error in the translatio (and possibly twist) (team 2875 had it on our 2022 robot). We added in software-based drift correction through the use of two PID's in teleop, one for X position and one for Y position. You can simply run the pure joystick inputs through the PIDs before feeding through the rest of swerve drive. (NOTE: you can only accomplish this if with a VERY accurate accelerometer or using a wheel-speed based odomety (which I will go over in the next tutorial), if you wish to try your hand at some drift correction, you can attempt to fix any angular drift you might have (this doesn't need odometry/accelerometer; only a gyroscope))
5. Extensive SmartDashboard/Shuffleboard Integration - This goes for the entire tutorial but can be especially useful in the drive train. You can organize different tabs on shuffleboard, some with debug information, others customized to your drivers needs. You can put wheel speeds, robot speeds, throttle, and any other value you with onto these tabs.