Math and Programming Behind Swerve


This will be my senior year in high school and I am completly fascinated by the complexities of true swerve driving and the capabilities of it. I would rate my math skills very high but my programming skills are pretty low, i’ve never had instruction in programming. I am highly interested in learning how to program swerve drive and what it truly is. If you can help me out I would greatly appreciate it!:smiley:


This depends on how the modules are driven. For the sake of this post, I am assuming that you are talking about independently steered and driven modules. There are several different ways to calculate the angles and speeds of each wheel, but the goal is to first find the point around which the robot is turning (it can be inside or outside the frame perimeter), then make sure all the wheels are tangent to a circle that has a center point on the robot turning point, and intersects the wheel. Then, based on the circumference of these circles, you must adjust the speed of each wheel so that the one that travels the farthest distance travels faster (like a differential on a car).

This can be calculated by adding the rotation to the xy movement for each wheel. To find the xy movement, you must first convert the xy to polar coordinates. r = sqrt(x^2+y^2), and theta = atan2(y, x) (note- some software reverses the order of the arguments for atan2 like excel, but LabView, Java, and C++ are like this)

R will be your wheel speed, and theta will be the wheel angle.
Then, you need to normalize r, because it can have a max value of sqrt(2) (at 1,1)
Then, it needs to be converted back to xy so that it can be added to the rotational part.

To find the rotational part, you need to know where the wheels are located relative to the center of the robot like on a coordinate plane so you have x and y coordinates for each wheel. Then, you need to convert these to polar coordinates, add 90 degrees to theta (so that wheels will be like /)
and multiply r by the desired rotation (this will cause the wheels farther from the center to spin faster so they can travel farther). Take these modified coordinates, convert to xy, add to the translation part, convert back to polar, and you will have your setpoints for wheel speed and angle.

To see a good implementation of programming swerve in java/c++ see Ether’s post and look at the very top and very bottom ones.

As for LabView, check out 1094’s post here.


Don’t double post. Your answer is in the other thread


I believe our programming team is finishing up a code revue and will post our 2013 swerve code some time this summer.


sorry, i couldn’t figure out where the post had went but i found it now. I greatly appreciate your answer in the other one and i’ll take it down now to prevent confusion :slight_smile:


In case you hadn’t found this yet, see this post for our 2011 drive code (in LabVIEW library form): paper: Team 1640 "Pivot Drive" LabVIEW Library

We are putting the finishing touches on releasing our competition season code, which includes the drive code. I’ll be posting the link to the GitHub repository as well as a zip file (if it isn’t too large), hopefully before we leave for IRI.


I believe that Ether has posted a whitepaper that goes through the math and pseudocode for a true swerve drive. It’s sounding that that’s really what you want.

I greatly appreciate your answer in the other one and i’ll take it down now to prevent confusion

Would you mind re-posting the answer, for posterity’s sake? If it’s a greatly appreciated answer, chances are that someone else would also be able to greatly appreciate it. :slight_smile:


1640s swerve repository is here

Go for it.


Here are links to the documents:

Derivation of the equations:

How to use the equations:

Excel spreadsheet swerve calculator:

Miscellaneous additional documents: