Updated the code to determine shooter angle, updates to climber code, added bangbang controll