Re: swerve math quiz
 

Generalizing this exercise for any number of wheels N in any configuration:

Let XY_robot be a standard Cartesian coordinate system fixed with respect to the robot, with +Y axis pointing in the robot "forward" direction, and origin located at point C (which can be anywhere within the robot frame, or even outside the frame of the robot).

Let (X_i,Y_i), i=1..N, be the coordinates of the robot's N wheels in the XY_robot coordinate system.

Then, the wheel tangential speeds and steering angles for clockwise rotation of the robot about the point C at ω radians/sec are given by:

speed_i = ω√(X_i²+Y_i²)

angle_i = atan2(Y_i,-X_i)

...where angle_i is zero at the "straight forward" position and is measured clockwise therefrom with range ±π