Added SmartDashboard reporting

for Autonomous Mode as well :)
Only test mode is left.