Once scoring on the medium junction is possible.
Make a trajectory from high scoring position to medium scoring position.
If Retreat is enabled, put a much shorter timeout on the Wait For Roadrunner steps, and return a different value if that's what times out.
Then instead of parking, go to the medium goal and continue cycling.
This is complicated, because you might retreat at three different places in the scoreAndFetch subroutine, and you'll need to resume cycling based on whether the robot has a cone at the time of retreating.
Once scoring on the medium junction is possible. Make a trajectory from high scoring position to medium scoring position. If Retreat is enabled, put a much shorter timeout on the Wait For Roadrunner steps, and return a different value if that's what times out. Then instead of parking, go to the medium goal and continue cycling.
This is complicated, because you might retreat at three different places in the
scoreAndFetch
subroutine, and you'll need to resume cycling based on whether the robot has a cone at the time of retreating.