Velocity PID still broken. Unit tests for commands.

This commit is not tested on the bot. Unit tests now should be called
clicking on a button in the SmartDashboard (no Joystick buttons). The
logic of the speed maintaining controllers is still broken. Maybe
PIDController is not the best choice for that.