Nice work Jon.
I'll check the code one of these days.
In my university our diff robots are so small and cheap they have steppers rather than encoders

One thing I can think can expand your stack is a holonomic controller:
lin_spd=cos(robot_theta) *vx+sin(robot_theta)*vy;
ang_spd=-( 1/d )*sin(robot_theta) *vx+(1/d)*cos(robot_theta)*vy;

d being the distance from the actual drive center (the pivoting point of the platform) along the longitudinal axis of the robot (the forward movement axis).
Then going from (lin_spd, ang_spd) to (wheel_1, wheel_2) is trivial.

Relevant article:
Bilateral Teleoperation of a Formation of Nonholonomic Mobile Robots
Under Constant Time Delay
Oscar M. Palafox and Mark W. Spong
(can't attach here)


Best regards