Added logic for shooting height in teleop

only about 4 commits left :)