Finished servo, added camera initializer in robotInit. Also added SmartDashboard support