Java Code - Team 4716 FRC