Added smartdashboard test code
Pulled master code
Added smartdashboard test code