Autonomous chooser now implemented, option for each corner of the pyramid added. Also some clean up.