SmartDashboard code for servo

Added some SmartDashboard code to display the servo's X and Y.