Improve/fix fetcher lockout while lever on stack

[robot256]

Right now useOffset locks out fetcher commands, but there are a few issues:

1. It causes all commands during useOffset to be discarded, not just delayed.
2. It applies to raising from ground height, when there is no stack.
3. It only applies until horizontal, not until the cone is clear of the stack.
4. It doesn't care whether the grabber is open or closed.

We can fix all of these by adding a new flag in scorer, "pauseFetcher", and two new variables, "unpauseFetcherAngle" and "pausedFetcherCommand".

• [x] pauseFetcher(stackHeight) will set fetcherPaused to true, set pausedFetcherCommand to fetcher.extensionSetPoint, and unpauseFetcherAngle to the required STACK_X_CLEAR preset angle.
• [x] unpauseFetcher() will clear fetcherPaused, set fetcher.extensionSetPoint to pausedFetcherCommand, and set unpauseFetcherAngle to +pi.
• [x] pauseFetcher(stackHeight) should be called by leverToggleHome and leverGoHome when starting in a stack1,2,3,4 high and going inside IF the grabber is closed.
• [x] unpauseFetcher() should be called by any other lever command.
• [x] In scorer.act, when lever servo position is greater than unpauseFetcherAngle, also call unpauseFetcher().
• [x] In controlFetcher, discard each command of fetcherPaused is true.
• [x] In fetcherGoToPreset and setFetcherPosition (NOT controlFetcher):
• IF fetcherPaused is false, send the new command straight to fetcher.
• IF fetcherPaused is true, AND the new command value is LESS than the current command value, then the new command should be stored in pausedFetcherCommand.

[robot256]

Question: should we also lock out the mecanum drive joysticks while fetcher is locked?

[robot256]

Implementing this ticket will prevent Auto from knocking over the stack during the giveUp(tm) sequence.

[TheDeveloper101]

i'll do this one as well

[robot256]

Looks ready to test!

[robot256]

Something is going on and it's not pausing like it should when the grabber is closed. It's possible that it's only pausing when the grabber is open.

[robot256]

Okay, the offset is taking place very briefly and the fetcher is slow to respond, so it does almost nothing. But at the same time, the Auto seems to be able to pick up cone #4 just fine? So it might be doing its job.