Software · Drivetrain

Swerve Kinematics Lab

Drive a swerve robot with two sticks and watch the math. The sticks become one ChassisSpeeds, kinematics turns it into a speed and angle for each of the four wheels, and the wheels move the robot. New here? Press Walkthrough.

Field (blue half)

Robot velocity
Left stick · drive
ChassisSpeeds (robot-relative)
vx m/s–
vy m/s–
ω rad/s–
Input: on-screen sticks
Right stick · turn

Modules

Wheel command Before desaturation
ModuleAngle (rad)Asked (m/s)Sent (m/s)Steer travel (rad)

Order is FL, FR, BL, BR, the same as WPILib's SwerveDriveKinematics. Asked is straight out of kinematics. Sent is after desaturation and optimization; a minus sign means the wheel spins backward. Steer travel adds up how far each wheel has turned since the last reset.

What the robot is doing

Heading θ–
Measured vx m/s–
Measured vy m/s–
Measured ω rad/s–

Measured speeds come back from the wheels (forward kinematics). When they don't match the ChassisSpeeds you asked for, the robot is not doing what the driver wanted.

The state RobotContainer asks for

From stick to ChassisSpeeds, step by step

This is MWLib's calculateSpeedsBasedOnJoystickInputs(): deadband each axis, flip the sign (stick forward reads negative, and WPILib +y is left), multiply by the max rate, then a slew limiter on vx and vy. Read more on the Swerve and Operator Interface pages.

Things to try

  1. Press Turn robot +π/2 so the robot faces left, then push the left stick forward in Field-centric and in Robot-centric. Which one would a driver standing at the blue wall want?
  2. Spin in place with only the right stick. All four wheels point along a circle around the center, and the table shows the same speed on each: ω × 0.269 m.
  3. Turn on Show the vector sum, then drive forward and spin at the same time. The front-left and back-left wheels end up different lengths. Why?
  4. Full forward plus full spin. The fastest wheel is asked for over 7 m/s. Look at the dashed arrows, then turn off Desaturate and watch the path bend.
  5. With Optimize on, flick the stick from full forward to full back. No wheel turns; they all spin backward. Turn Optimize off, lower Steering speed to 4 rad/s and do it again.
  6. Turn off both Optimize and Cosine scaling and reverse quickly. The robot lurches the wrong way while the wheels turn. Cosine scaling is the fix.
  7. Pick Rotation lock and strafe around the hub. The heading controller spins the robot so the front always faces the hub, and the right stick does nothing.
  8. Turn off Discretize, then drive straight while spinning hard in Field-centric. The path drifts to one side. That drift is what ChassisSpeeds.discretize removes.