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
-- 

*Eng. Claudio Carbone
Embedded Systems Design*

P.IVA: 11688471009
tel: +393809017424
email: Send email
<mailto:cla_carbone@tiscali.it>My linkedin profile 
<http://it.linkedin.com/in/embeddedesign/en>

My Portfolio
My portfolio site <http://www.fusioncoredesign.it/>



-- 

*Eng. Claudio Carbone
Embedded Systems Design*

P.IVA: 11688471009
tel: +393809017424
email: Send email
<mailto:cla_carbone@tiscali.it>My linkedin profile 
<http://it.linkedin.com/in/embeddedesign/en>

My Portfolio
My portfolio site <http://www.fusioncoredesign.it/>