Added PidDriveController, Added Vision Template, Cleaned up Code