Much command restructuring. Added SmartDashboard support. Many more fixes.