Unit 7 · Lesson 7

WPILib Swerve Classes

You've been using these classes since Lesson 3. This lesson makes you fluent in all of them — not just the methods you've already called, but the full API surface of ChassisSpeeds, SwerveDriveKinematics, SwerveModuleState, Pose2d, Rotation2d, and Translation2d. Knowing the full API means fewer trips to the documentation and faster debugging.

By the end of this lesson, you will:

Use all major factory methods on Rotation2d and Translation2d and explain when to prefer each
Describe the difference between Pose2d.plus(Transform2d), Pose2d.relativeTo(), and Pose2d.transformBy()
Call ChassisSpeeds.fromFieldRelativeSpeeds() and explain its geometry
Call SwerveDriveKinematics.toChassisSpeeds() (forward kinematics) and state when it's used
Explain the difference between SwerveModuleState and SwerveModulePosition and which is used where
Use Pose2d.toString() and geometry math for SmartDashboard debugging

Rotation2d — The Angle Wrapper

Rotation2d is WPILib's type-safe angle object. It stores an angle internally in radians but provides factory methods in whatever unit you prefer. Using it instead of raw doubles prevents unit confusion and enables geometry operations.

Rotation2d — construction

Rotation2d.fromDegrees(deg)most used

Convert degrees → Rotation2d. Use when your angle comes from the Pigeon 2 (which reports degrees) or when you're hard-coding a human-readable angle like 90°.

Rotation2d.fromRotations(rot)Phoenix 6

Convert rotations (0.0–1.0) → Rotation2d. Use when reading from a CANcoder or working with Phoenix 6 position values. Avoids the ×2π conversion.

Rotation2d.fromRadians(rad)

Convert radians → Rotation2d. Use when your angle comes from WPILib geometry calculations or trigonometry.

new Rotation2d() / new Rotation2d(0)

Zero rotation (pointing forward). Idiomatic way to say "no rotation" in a SwerveModuleState or Pose2d.

Rotation2d — reading

.getDegrees()

Returns angle in degrees as a double. Use for SmartDashboard publishing (human-readable) and for logging.

.getRadians()

Returns angle in radians. Use when passing to Math.sin(), Math.cos(), or any standard Java math function.

.getRotations()

Returns angle in rotations. Use when commanding Phoenix 6's PositionVoltage or comparing to CANcoder values.

.getCos() / .getSin()

Returns the cosine or sine of the angle. Used in rotation matrix math — for example, in field-oriented control's velocity rotation.

Rotation2d — arithmetic

.plus(other) / .minus(other)

Add or subtract angles. Result is normalized to [−π, π]. Rotation2d.fromDegrees(350).plus(Rotation2d.fromDegrees(20)) returns 10° — the wrap is handled automatically.

.unaryMinus()

Returns the negated angle. Used when converting Pigeon 2 yaw to WPILib heading: Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()).unaryMinus().

.rotateBy(other)

Rotates this angle by another — same as .plus() but semantically clearer when composing 2D transforms.

Translation2d and Pose2d — Position on the Field

Class	What it represents	Common usage in swerve
Translation2d(x, y)	A 2D point or displacement in meters. No heading.	Module positions in kinematics constructor; vision target positions; scoring location waypoints
Pose2d(x, y, rotation)	A position + heading. The robot's full state on the field.	Odometry output; auto starting positions; PathPlanner waypoints; AprilTag pose
Transform2d(translation, rotation)	A relative change in position and heading.	Moving from camera pose to robot pose; predicting robot position after a known motion

Geometry operations — common patterns in swerve

// ── Pose arithmetic ───────────────────────────────
Pose2d robotPose = getPose(); // e.g. (3.0, 2.0, 45°)

// Where is the target relative to the robot?
Pose2d targetField = new Pose2d(5.0, 2.0, new Rotation2d());
Pose2d targetRelative = targetField.relativeTo(robotPose);
// targetRelative.getX() = forward distance to target
// targetRelative.getY() = lateral offset to target

// Where will the robot be after a known transform?
Transform2d motion = new Transform2d(
  new Translation2d(1.0, 0.0), new Rotation2d()); // 1m forward
Pose2d predicted = robotPose.plus(motion);

// Distance from robot to target (for approach logic)
double dist = robotPose.getTranslation()
  .getDistance(targetField.getTranslation());

// ── ChassisSpeeds API ─────────────────────────────
// Constructor (robot-oriented)
ChassisSpeeds robotSpeeds = new ChassisSpeeds(2.0, 0.5, 0.3);

// fromFieldRelativeSpeeds (field-oriented — Lesson 9)
ChassisSpeeds fieldSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 0.5, 0.3, getHeading());

// toChassisSpeeds (forward kinematics)
ChassisSpeeds actual = m_kinematics.toChassisSpeeds(
  m_fl.getState(), m_fr.getState(),
  m_bl.getState(), m_br.getState());

// ── State vs Position distinction ─────────────────
SwerveModuleState state = m_fl.getState(); // velocity + angle
SwerveModulePosition pos = m_fl.getPosition(); // distance + angle

← click a highlighted token

The State vs. Position Distinction — A Common Confusion

Both SwerveModuleState and SwerveModulePosition contain an angle. But the other field is completely different, and using the wrong one in the wrong context produces subtle bugs with no compiler error.

Class	Speed/Distance field	Angle field	Used by	Analogous to
SwerveModuleState	`speedMetersPerSecond` — instantaneous velocity	`angle` (Rotation2d)	`setDesiredState()`, `toChassisSpeeds()`, SmartDashboard	Speedometer reading + compass
SwerveModulePosition	`distanceMeters` — accumulated travel since reset	`angle` (Rotation2d)	`SwerveDriveOdometry.update()`, `resetPosition()`	Odometer reading + compass

⚠️ Passing the wrong type to odometry causes silent positional drift

If you accidentally pass a SwerveModuleState array where the odometry expects a SwerveModulePosition array (or vice versa), Java's type system will catch it at compile time — they are different types. But if you pass position data from before a resetPosition() call, the odometry receives stale data and starts from the wrong baseline. Always call getModulePositions() immediately before every update() and every resetPosition() call — never cache the positions array across multiple loop cycles.

Complete Class Reference

A quick-reference card for every WPILib swerve-related class. Bookmark this mentally — these are the methods you'll reach for repeatedly throughout the season.

SwerveDriveKinematics

toSwerveModuleStates(ChassisSpeeds)inverse

Convert desired chassis motion → 4 module states. The core of the drive loop. Returns array in constructor-argument order.

toChassisSpeeds(SwerveModuleState...)forward

Convert 4 actual module states → chassis velocity. Forward kinematics. Used by PathPlanner and for speed verification.

desaturateWheelSpeeds(states, max)static

Scale all four speeds proportionally so fastest ≤ max. Always call after toSwerveModuleStates().

ChassisSpeeds

new ChassisSpeeds(vx, vy, omega)

Direct construction for robot-oriented speeds. Lesson 8's approach.

fromFieldRelativeSpeeds(vx,vy,ω,heading)Lesson 9

Convert field-relative input → robot-relative speeds using the gyro heading rotation matrix.

discretize(speeds, dtSeconds)always use

Correct for discrete 20 ms sample period. Call before toSwerveModuleStates().

SwerveModuleState

SwerveModuleState(speed, angle)

Instantaneous target or measurement: speed in m/s, angle as Rotation2d.

optimize(desired, current)static

Minimize steering travel. Always call in setDesiredState(). Mutates a copy — doesn't modify the original.

.speedMetersPerSecond / .angle

Public fields (not getters). Read directly: state.speedMetersPerSecond.

SwerveModulePosition

SwerveModulePosition(distance, angle)

Accumulated distance in meters + current angle. Used exclusively for odometry.

.distanceMeters / .angle

Public fields. distanceMeters increases as the wheel rolls forward, decreases rolling backward.

Pose2d

new Pose2d(x, y, rotation)

Field position in meters + heading. The standard FRC robot state representation.

.getTranslation() / .getRotation()

Extract the position or heading component.

.relativeTo(referencePose)

Compute this pose's position and angle relative to another pose. Use to find how far/rotated the robot is from a target.

.plus(Transform2d)

Apply a transform to this pose to get a new pose. Use for predicting future position.

SwerveDriveOdometry

update(heading, positions[])

Integrate new sensor data. Call every periodic() loop. Returns updated Pose2d.

resetPosition(heading, positions[], pose)

Declare current position. Called in autonomousInit() and by PathPlanner at path start.

getPoseMeters()

Returns current estimated Pose2d. The main output of odometry — everything else reads from here.

Knowledge Check

1. You need to know if the robot is within 0.5 meters of a scoring target at (8.2, 4.1) on the field. The robot's current pose is (7.8, 3.8). Which method call gives you the distance?

A robotPose.relativeTo(targetPose).getX()
B robotPose.getTranslation().getDistance(targetTranslation) — returns Euclidean distance between two Translation2d points in meters.
C robotPose.minus(targetPose).getNorm()
D Math.sqrt(Math.pow(8.2-7.8,2)+Math.pow(4.1-3.8,2)) — correct math, but WPILib already provides this.

2. SwerveModuleState.optimize() takes (desired, current) as arguments. If you accidentally swap them and pass (current, desired), what happens?

A Java throws an argument type error at compile time.
B The optimizer treats the current angle as the desired target and optimizes toward it — effectively commanding the module to hold its current angle and setting the drive speed based on the "desired" angle's context. The wheels mostly stop steering and the drive speed becomes wrong, producing erratic behavior with no exception.
C The module steers in the wrong direction but corrects itself on the next loop cycle.
D No effect — optimize() is commutative.

3. Rotation2d.fromDegrees(350).plus(Rotation2d.fromDegrees(30)) — what does this return?

A Rotation2d.fromDegrees(380) — angles are additive
B Rotation2d.fromDegrees(20) — Rotation2d automatically wraps to [−180°, 180°]. 350° + 30° = 380° → wraps to 20°.
C Rotation2d.fromDegrees(-10) — the negative wraps differently
D An exception — Rotation2d doesn't support arithmetic operations

💪 Practice Prompt

Explore the Full API in SmartDashboard

In your drivetrain's periodic(), add these four lines: publish getPose().getX(), getPose().getY(), getPose().getRotation().getDegrees(), and the forward-kinematics result from getChassisSpeeds().vxMetersPerSecond. Drive the robot and confirm each updates correctly. The forward-kinematics vx should approximately match what you command in drive().
Add a test that computes the distance from the robot's current pose to a fixed point (e.g., the center of the field at (8.23, 4.0)). Publish this distance to SmartDashboard as "Drive/DistanceToCenter". Drive around and watch it change. This is the building block of proximity-based autonomous triggers.
Verify ChassisSpeeds.discretize() is in your drive() method. Remove it temporarily and drive a slow forward+rotation arc — you should see slight curvature. Re-add it and repeat. The path should straighten. If you can't perceive the difference at your test speed, try a very slow speed (0.1 m/s) with high rotation (1.0 rad/s).
Stretch goal: Write a unit test (or a console check in teleopInit) that verifies: Rotation2d.fromDegrees(90).plus(Rotation2d.fromDegrees(200)) equals approximately Rotation2d.fromDegrees(-70). Then verify Rotation2d.fromDegrees(-90).unaryMinus() equals 90°. These tests confirm your angle arithmetic understanding before you need it in field-oriented control.