[Patrickwhite]

Interesting! I haven't taken too deep a look yet, but one thing you may want to look into doing is creating a class (or classes) for constants that you may need to change often. For example, in our code, instead of

Victor armJoint = new Victor(4);

we would use

Victor armJoint = new Victor(ElectricalMap.kVicArmJoint);

It's longer but makes the code more consistent, and easier to maintain.