Removed SmartDashboard-related output.
Improve some comments, add some TODOs