Add SmartDashboard functionality and debug support