Added SmartDashboard displays for debugging