Tested and added SmartDashboard helpers