WPILib’s SwerveDriveKinematics class supports an arbitrary number of wheels >= 2. The WPILib implementation uses a generalized form of the derivation in section 12.7 “Swerve drive kinematics” in https://file.tavsys.net/control/controls-engineering-in-frc.pdf.

I’ll be using the following coordinate axes:

Each module has two actuators and thus two states: velocity of the module along the x axis (v_{1x} for module 1), and velocity of the module along the y axis (v_{1y} for module 1). Since your third wheel is just there to keep the robot from tipping over, your inverse kinematics (skipping over the steps shown in the book) would be:

\begin{bmatrix}
v_{1x} \\
v_{1y} \\
v_{2x} \\
v_{2y}
\end{bmatrix} =
\begin{bmatrix}
1 & 0 & -r_{1y} \\
0 & 1 & r_{1x} \\
1 & 0 & -r_{2y} \\
0 & 1 & r_{2x}
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y \\
\omega
\end{bmatrix}

where r_{1x} is the displacement from the center of rotation ^\dagger to the wheel module along the x axis (same logic applies for other module and axis combos), v_x is the chassis x velocity, v_y is the chassis y velocity, and \omega is the chassis angular velocity. You could multiply that out to get four separate equations for the velocity components

\begin{bmatrix}
v_{1x} \\
v_{1y} \\
v_{2x} \\
v_{2y}
\end{bmatrix} =
v_x
\begin{bmatrix}
1 \\
0 \\
1 \\
0
\end{bmatrix} +
v_y
\begin{bmatrix}
0 \\
1 \\
0 \\
1
\end{bmatrix} +
\omega
\begin{bmatrix}
-r_{1y} \\
r_{1x} \\
-r_{2y} \\
r_{2x}
\end{bmatrix}

v_{1x} = v_x -\omega r_{1y}

v_{1y} = v_y + \omega r_{1x}

v_{2x} = v_x -\omega r_{2y}

v_{2y} = v_y + \omega r_{2x}

but the matrix notation makes it easier to do the forward kinematics (i.e., going from wheel speeds to chassis speed for use with odometry). You invert the 4x3 matrix (linear algebra libraries have functions that make this easy) then multiply your wheel speed vector by it to get your chassis speed vector.

To convert the swerve module x and y velocity components to a velocity and heading, use the Pythagorean theorem and arctangent respectively. Here’s an example for module 1.

v_1 = \sqrt{v_{1x}^2 + v_{1y}^2}

\theta_1 = \tan^{-1}\left(\frac{v_{1y}}{v_{1x}}\right)

or in Java,

```
double v1 = Math.hypot(v1x, v1y);
double heading1 = Math.atan2(v1y, v1x);
```

WPILib’s swerve bot example has some other useful stuff like module angle optimization (negate the wheel velocity when necessary to minimize the change in module rotation). If you decide to implement this, I suggest starting from that example and tweaking the number of modules. The motor and sensor configs are wrong for your drivetrain, but the math and plumbing is there.

^\dagger The driver can usually move the center of rotation around to do pivots around different points.