Skip to content
GitHubYouTubeInstagram

Four wheels, any direction.stick → wheel speeds and angles → a pose on the field

Our robots drive on swerve: four wheels that can each point any direction and spin at any speed. This course takes one module apart, then puts four of them together and follows the math that turns a joystick push into motion, and motion back into "the robot is here."

5 chapters · about 29 minutes

  1. 01 · 5 minInside a swerve moduleTake an SDS MK5n apart, one part at a time.
  2. 02 · 6 minFour wheels, one motionTurn a joystick push into four wheel speeds and angles.
  3. 03 · 7 minOdometry: dead reckoningAdd up tiny wheel movements to track where the robot is.
  4. 04 · 6 minInside the gyroHow a chip with no spinning parts measures rotation.
  5. 05 · 5 minField-centric vs robot-centricDrive it both ways and feel why drivers want field-centric.
Chapter 1 of 5 5 min

Inside a swerve module

Take an SDS MK5n apart, one part at a time.

A swerve drive has four modules, one at each corner. Each module has two motors: a drive motor that spins the wheel, and a steering motor that points it. Point all four wheels the same way and the robot slides in that direction without turning. Point them in a circle and it spins in place. Anything in between, it can do both at once.

The module on our 2026 robots is the MK5n from Swerve Drive Specialties (SDS). It has a 5" × 7.5" footprint, a 4" wheel, a Kraken X60 driving and a Kraken X44 steering, and it's all gears inside: no belts. Take one apart below.

LabTake an MK5n apart

Drag to orbit, scroll to zoom. Click any part, or pick one from the list. Then spin the motors and watch which gears move.

Scroll here to load the 3D model.
Drive pathSteering pathSelected part(path colors show with "Show power paths")
PartsPick a part to see what it does.
Wheel points0°
Wheel turns0.00
Steering motor turns0.0
Drive motor turns0.0
Drive ratio7.03 : 1

The gears turn at one fifth of the motor speeds shown, so you can follow the teeth. Tooth counts are SDS's; gear sizes and positions are our fit (see the note at the end of the chapter).

Two paths for power

Everything inside the housing is one of two gear trains. Switch the lab to Show power paths: the orange parts carry the drive motor's power down to the wheel, and the blue parts carry the steering motor's.

The MK5n with its housing see-through and only the drive path in orange: the Kraken X60's pinion, the idler, the drive double gear, the 25-tooth gear and the bevel shaft down to the 30-tooth bevel on the wheel.
Drive path. Kraken X60 down to the wheel.
The MK5n with its housing see-through and only the steering path in blue: the Kraken X44's pinion, the steering double gear and the main steering gear that turns the wheel mounts.
Steering path. Kraken X44 to the wheel mounts.

Drive (orange): the X60's pinion turns a 43-tooth idler, which turns the 54-tooth half of the drive double gear. That gear sits right on the steering axis. Its 32-tooth half drives a 25-tooth gear on a vertical shaft, and a 15-tooth bevel gear on the bottom of that shaft turns the 30-tooth bevel bolted to the wheel. Multiply the stages and you get the gear ratio:

54/12 × 25/32 × 30/15 = 7.03 : 1  (R1)

Changing the ratio only means swapping the pinion (12, 14 or 16 teeth for R1, R2 or R3) and sliding the shift plate so the idler meets it. Nothing else comes apart. Try the ratio buttons in the lab and watch the idler move.

Steering (blue): the X44's pinion drives the 41-tooth half of the steering double gear, and its 7-tooth half drives the main steering gear, which turns the whole bottom of the module on a thin 4" bearing. SDS gives the ratio as 287 : 11, about 26 steering-motor turns for one turn of the wheel. The encoder sits on top, reading a magnet glued into the main steering gear. Because it's an absolute encoder, the robot knows which way every wheel points the moment it powers on.

The magnet is glued in at whatever angle it landed, so each module's CANcoder needs a magnet offset. Point all four wheels straight forward, with the bevel gears facing the same side on every module, and save each encoder's reading as that module's zero. A module whose offset is a few degrees off drives crooked and fights the other three; one that's 180° off drives backward. A bad offset is the most common swerve bring-up bug.

Why the drive gear is on the steering axis

The drive double gear spins around the same axis the module steers around, so the design is coaxial. Because drive power comes down the middle, nothing winds up, and the wheel can steer forever in either direction.

There is one side effect, and it's easy to see in the lab. Set the drive motor to 0 and move only the steering motor. The wheel still rolls. The 25-tooth gear rides around the drive gear as the module steers, like a planet around a sun, so steering alone turns it. This is called coupling. Odometry would read that as the robot driving when it didn't.

To cancel it, the drive motor has to turn a little extra whenever the module steers: 54 ÷ pinion teeth turns for every full turn of the module. That's 4.5 for R1 and 3.375 for R3. It's the number CTRE's swerve code calls CouplingGearRatio. Tick Cancel coupling in the lab and steer again: now the wheel holds still.

The one thing to remember A module is two gear trains sharing one axis. The drive motor spins the wheel, the steering motor points it, and the encoder on top reports where it's pointing.

The housing, plates, wheel mounts, bearing and wheel are SDS's own MK5n CAD. SDS's CAD has no gears, so the gear train is built from their assembly guide and gear-ratio table: the tooth counts are real, but tooth sizes and exact gear positions are our best fit inside the housing. For the real CAD, download SDS's MK5n Layout A and B models from the same page.

Next: Four wheels, one motion ↓

Chapter 2 of 5 6 min

Four wheels, one motion

Turn a joystick push into four wheel speeds and angles.

The driver doesn't steer four wheels. They ask for one motion: "go that way, this fast, and turn this much." That request is three numbers, the chassis speeds: forward speed vx, sideways speed vy, and turning speed ω (omega). Swerve kinematics turns those three numbers into a speed and an angle for each of the four wheels.

The idea fits in one sentence: every wheel moves with the robot, plus its share of the robot's spin. If the robot only slides, all four wheels point the same way at the same speed. If it only spins, each wheel points sideways to the line from the center, like a car going around a roundabout. When the robot does both, each wheel adds the two arrows together.

LabFour wheels, one motion

Drag the stick to slide, and the slider to turn. Try sliding and turning at once, then push both to the limit.

Slide part (the same for every wheel)Spin part (sideways to the center)What the wheel does (the sum)
Slide: vx, vy
WheelAngleSpeed
Asked for0.0, 0.0 m/s, 0.0 rad/s
Desaturationnone

Simplified: wheels reach their speed instantly and steer at a fixed 12 rad/s. Top wheel speed 4.5 m/s; modules 0.28 m from the center.

For a wheel at position (x, y) from the robot's center, the math is just those two arrows added:

wheel vx = vx − ω · y
wheel vy = vy + ω · x

The speed is the length of that arrow and the angle is its direction. That's all WPILib's SwerveDriveKinematics.toSwerveModuleStates() does, four times over.

When a wheel can't keep up: desaturation

Push the stick all the way and turn hard at the same time, and the lab asks one wheel to go faster than its motor can. The outside wheels always need the most, because they add the slide and the spin in the same direction. If the code just capped that one wheel, the four wheels would stop agreeing and the robot would curve off somewhere the driver didn't ask for. Instead it desaturates: every wheel is scaled down by the same factor. The robot goes slower, but in exactly the direction requested.

Never turn more than 90°: module optimization

A wheel pointing straight backward and spinning backward pushes the same way as one pointing forward and spinning forward. So if a wheel would have to steer more than 90° to reach its new angle, the code turns it the short way and runs the drive motor in reverse instead. WPILib does this with state.optimize(currentAngle) on a SwerveModuleState (the static SwerveModuleState.optimize(state, angle) was deprecated in 2025), and it's why the wheels in the lab never swing all the way around. Some rows in the table show a negative speed: those wheels are driving backward on purpose.

The one thing to remember Each wheel = the robot's slide + the robot's spin at that corner. If any wheel can't keep up, all of them slow down together.

Next: Odometry: dead reckoning ↓

Chapter 3 of 5 7 min

Odometry: dead reckoning

Add up tiny wheel movements to track where the robot is.

The robot has no GPS. To know where it is, it starts from a known spot and keeps adding up how far it has gone and in which direction. That's odometry, also called dead reckoning.

Every module reports two numbers: how far its wheel has rolled (from the drive motor's encoder, divided by the gear ratio, times the wheel's circumference) and which way it's pointing (from the steering encoder). Many times a second, the robot compares those to the last reading and works backward from the wheels to the chassis. That's kinematics from chapter 2, run in reverse. The result is one small twist: "since last time I moved this far forward, this far left, and turned this much." Adding each twist to the pose is what tracks the robot across the field.

LabDead reckoning

Let it run, then add some slip or a bump. Watch the purple estimate peel away from the real robot.

Where the robot really isWhere odometry thinks it is
Heading comes from
Position error0 cm
Heading error0.0°
Time since reset0 s
Odometry updates0 (250 per second)

Simplified: steering angles are read perfectly and slip is random noise on distance. The update runs at 250 Hz like our 2026 code; the gyro drifts at the Pigeon 2's rated 0.4°/min.

What happens each update

  1. Read each module's position: distance rolled and wheel angle. WPILib calls this a SwerveModulePosition.
  2. Subtract the last reading to get how far each wheel moved since then.
  3. Run kinematics backward to get the chassis's twist: forward, sideways and turn. Four wheels give eight numbers for three unknowns, so this is a best fit. When the wheels disagree, as they do when one slips, the fit averages the disagreement in.
  4. Replace the turn with the gyro's. WPILib's odometry throws away the wheels' idea of how much the robot rotated and uses the gyro's change in heading instead. Wheels are bad at measuring rotation because turning makes them scrub sideways. Flip the lab to The wheels to see how much worse it gets.
  5. Apply the twist to the pose with the pose exponential. It assumes the robot moved along a smooth arc since the last update, not in a straight line, which matters when it's driving and turning at once.

CTRE's swerve code runs this on its own thread, 250 times a second when the drivetrain is on a CAN FD bus (a CANivore), or 100 on plain CAN. Our 2026 code sets 250 Hz explicitly. More updates means each arc is shorter, so assuming it's a perfect arc costs less.

Why it drifts

Every twist has a little error in it, and odometry only ever adds. Nothing ever subtracts the error back out, so it builds up. That's drift. The big sources:

  • Slip. A wheel that spins on the carpet reports distance the robot never traveled.
  • The wrong wheel size. Tread wears and squishes, so a "4-inch" wheel isn't exactly 4 inches. Our 2026 competition code uses a wheel radius of 1.964" and the offseason robot's uses 1.978", instead of the nominal 2". A radius 1% wrong puts the robot a centimeter off for every meter it ends up from where it started. On the lab's loop that partly cancels out each lap, so watch the error grow and shrink as it drives.
  • Getting hit. Shove a robot sideways and its wheels skid. The pose moves, but the encoders barely notice. After one bump, odometry is wrong for the rest of the match.

That's why the robot also has cameras. Vision measures the pose directly from AprilTags on the field and pulls the estimate back to the truth. Our vision course covers that part. Odometry is smooth and fast but drifts. Vision doesn't drift but is noisy and sometimes missing. A pose estimator blends the two.

The one thing to remember Odometry adds up small wheel movements, with the turn taken from the gyro. It's only as good as its worst wheel, and errors never go away on their own.

Next: Inside the gyro ↓

Chapter 4 of 5 6 min

Inside the gyro

How a chip with no spinning parts measures rotation.

Odometry takes its heading from the gyro, so it's worth knowing what's inside one. On our robots it's a CTRE Pigeon 2, an IMU: a gyro, an accelerometer and a compass on one small board, with a processor that combines them. There's no spinning wheel inside. It's a MEMS chip: springs and weights etched into silicon, too small to see.

A gyro measures rate, not angle

Inside, a tiny proof mass is kept vibrating back and forth on springs, thousands of times a second. If the chip rotates while the mass is moving, the Coriolis effect pushes the mass sideways, at a right angle to its motion. The faster the rotation, the bigger the push. Electrodes on either side measure the sideways wiggle as a change in capacitance. The chip turns that into a rate: degrees per second right now.

LabInside a MEMS gyro

Drag the slider to turn the chip and watch the mass start to wiggle sideways. Stop turning and it goes straight.

Sideways wiggle (what the electrodes see)Gyro reading
Gyro reads0 °/s

The wiggle's size is the reading. Slowed down to 1.2 wiggles a second so you can see it; a real MEMS gyro vibrates thousands of times a second.

To get an angle, the robot has to integrate that rate: add up "degrees per second × seconds since the last reading", over and over. Turn at 90°/s for one second and you've added 90°. The heading is the area under the rate curve.

LabAdding up rate

Press Play, or drag the purple line. Watch the shaded area under the rate curve add up into the heading the robot turns to.

Rate the gyro measures (°/s)Heading = running total of the area
Rate right now0 °/s
Heading so far0°

The rate profile is made up for illustration, and the gyro here is perfect: no bias, no noise.

That's the same trap as odometry. If the reading is off by a tiny constant amount, called bias, the error gets added in every time and the heading slowly walks away. Gyro makers spend most of their effort keeping bias small.

LabTiny errors add up

Drag across the graph, out past one match. On the right, three robots try to drive 5 m straight ahead with that much heading error.

Pigeon 2 (0.4°/min)Pigeon 2, moved right after power-on (1°/min)A much worse gyro, made up for scale (0.5°/s)
GyroHeading errorOff target after 5 m

Log scale, so each line is straight: the error grows steadily with time. Drift rates are 0.4°/min and 1°/min from CTRE's Pigeon 2 guide, and 30°/min for the made-up bad gyro.

CTRE rates the Pigeon 2's drift at about 0.4° per minute while moving, and only 0.12° per hour sitting still. That's under a degree and a half across a whole match, small enough that odometry can trust it. The chip does it with factory temperature calibration (temperature is the main thing that moves the bias) and by re-measuring its own bias whenever it has been perfectly still for 4 seconds. A robot that powers on and starts moving right away drifts more, about 1° per minute, until it gets that still moment. So leaving the robot untouched for a few seconds after power-on is a good habit. There's nothing else to do: the Pigeon handles its own bias, and teams don't calibrate it. (The red line is a made-up gyro, 75 times worse than the Pigeon, there for scale.)

What the accelerometer is for

An accelerometer measures acceleration, and gravity counts. Sitting still, it reads 1 g pointing straight up, so it always knows which way is down. Tilt the robot and that 1 g shifts between the axes. That's how the Pigeon measures pitch and roll, and how it lets you mount it at any angle: its mount calibration uses gravity to work out how it's sitting on the robot.

LabWhat gravity can and can't tell you

Tilt the robot with Pitch and Roll and watch the readings change. Then turn it with the Yaw slider: the accelerometer doesn't notice.

AccelerometerX (fwd)Y (left)Z (up)
reads, g
Tilt it works out
Yaw it works outno idea

Simplified: a perfect accelerometer on a robot sitting still, so it feels only gravity.

Gravity points straight down no matter which way the robot faces, so spinning the robot flat on the floor changes nothing the accelerometer can feel. The accelerometer can correct pitch and roll, but never yaw. Yaw, the heading odometry needs, comes from the gyro alone. That's why gyro drift matters so much. (The Pigeon's compass could correct yaw in principle, but it has to be calibrated by hand and a robot full of motors and steel confuses it, so by default the Pigeon doesn't use it.)

One catch: the Pigeon combines gyro and accelerometer, so an acceleration that goes on for a long time can fool it. Spinning in place for a long time is the classic example, because a sensor far from the center feels a constant sideways pull. CTRE's advice is to mount the Pigeon near the robot's center of rotation.

The one thing to remember The gyro measures how fast the robot is turning, and adding that up gives heading, with a little drift. The accelerometer knows which way is down, so it fixes pitch and roll but can never fix heading.

Next: Field-centric vs robot-centric ↓

Chapter 5 of 5 5 min

Field-centric vs robot-centric

Drive it both ways and feel why drivers want field-centric.

So far the robot knows how to move and where it is. Now the driver has to tell it where to go. There are two ways to read a joystick, and swerve makes the difference obvious.

  • Robot-centric: pushing the stick forward moves the robot toward its own front. That's how a car works. Once the robot turns around, forward on the stick drives it toward the driver, and left becomes right.
  • Field-centric: pushing the stick forward moves the robot away from the driver, whichever way the robot is facing. The stick moves the robot on the field, and turning is a separate control.
LabDrive it both ways

Left thumb moves, right thumb turns (or click the field and use W A S D to move, Q / E to turn). Switch modes and try the same drive, then turn on Spin: only one mode lets you drive straight while spinning.

Move
Heading0°
Turn

Which wall you drive from only matters in field-centric. Robot-centric "forward" is always the robot's own front.

Stick asks for (field)0.0, 0.0
Sent to swerve (robot frame)0.0, 0.0

Simplified: a perfect gyro, and speed follows the stick with a short fixed lag instead of real acceleration. Top speed 3.5 m/s.

It's one rotation

Field-centric driving is a single line of math. The stick gives a direction on the field. The swerve kinematics from chapter 2 need a direction relative to the robot. So rotate the stick's arrow by minus the robot's heading:

robot vx =  field vx · cos θ + field vy · sin θ
robot vy = −field vx · sin θ + field vy · cos θ

WPILib's ChassisSpeeds.fromFieldRelativeSpeeds() does this, and so does CTRE's SwerveRequest.FieldCentric before it runs kinematics. The heading θ comes from odometry, whose heading comes from the gyro. This is why the gyro matters here: if the gyro drifts 10°, every "straight ahead" push goes 10° off, and the driver feels it at once.

Blue and red drivers

The field has one coordinate system, with the origin at the blue alliance wall. Everyone's pose, both alliances', is measured from there, so vision, autos and odometry all agree. But a driver on the red side is looking the other way. For them, "away" is the field's −x. So the code also needs the operator perspective: 0° for the blue wall, 180° for the red wall. Switch the lab to field-centric and Red driver: the field turns so you're looking from the red wall, and "field +x" now points at you. The stick still means "away from me", because the code adds the 180°.

The one thing to remember Field-centric = rotate the joystick by minus the heading (and by 180° on red). It only works as well as the heading, which is why the gyro matters.

Check yourself

Six questions from the whole course, chapters 1 to 5.

QuizSwerve
  1. 1. The drive motor is stopped and only the steering motor turns. Why does an MK5n's wheel still roll?

  2. 2. Driving and turning at once, kinematics asks one wheel for 5.4 m/s. The wheels top out at 4.5 m/s. What should the code do?

  3. 3. A wheel points forward, and its new target is 150° away. What does module optimization do?

  4. 4. Why does WPILib's odometry take the robot's rotation from the gyro instead of the wheels?

  5. 5. A robot powers on and starts driving right away. What does its Pigeon 2 do?

  6. 6. A blue driver has the robot facing them (heading 180°) and pushes the stick forward, in field-centric. Which way does the robot go?

That's the Swerve Drive course.

Next up: Vision & AprilTags, or head back to all courses.