Full-Battery Spoolup for Shooter

[ASingleToast]

Add state machine to add full spoolup when shooter as at max voltage

[gerth2]

One way to do this: Create a fininte state machine.

https://gm0.org/en/latest/docs/software/finite-state-machines.html

To do this, create a new enum for all the states the shooter could be in. Here's my quick list:

enum shooterState{
    STOPPED,
    ACCELERATE,
    STABILIZE,
    HOLD;
}

"During-state" defintiions:

In the STOPPED state, we should set the motor output to zero volts.

In the ACCELERATE State, we should set the motor output to the maximum possible voltage (~13 volts or so?)

In the HOLD and STABILIZE states, we should command a closed-loop velocity the same way we are doing today.

The shooter should only report it is spooled up in the HOLD state.

State Transitions:

From any state, if the shooter is commanded to stop, it should go to the STOPPED state.

If the shooter is commanded to turn on, go to the ACCELERATE State.

Stay in the ACCELERATE state as long as speed is less than ~100 RPM below the setpoint. Then, transition to the STABILIZE state. Make this ~100RPM a calibration.

Stay in the STABILIZE state until the actual RPM has been within ~50RPM of the setpoint for more than half a second. Then, transition to the HOLD state. Make both those thresholds tunable.

Stay in the HOLD state until two conditions are met simultaneously 1) The actual speed is below the stabilize/hold transition threshold (~50 RPM) 2) The derivative of the actual speed is positive (IE, we have started accelerating again)

This transition point should correspond to a ball leaving the shooter wheel. Transition back to the ACCELERATE state.

Update the "is spooled up" getter function to output true only if in the HOLD state, false otherwise.