Implemented SmartDashboard

Started smart dashboard, cleaned up various classes.