This is the code used in the Richmond competition plus a few if-statements if there are issues with drive, the shooter, the climber, or the camera