Purpose
The purpose of this addition is to have a new command to control teleop autonomously.

Project Scope

• Automate driving to source/back to a position where we can shoot
• Automate state transitions between automated driving, prime, shoot, etc.
• State machine to manage what the command is doing right now.

Initial Plan for States
These should transition linearly in a big circle in a 'perfect' cycle.

• Drive to source
  • Once a note is detected with vision or the robot reaches the source, switch to acquire note to go intake the note.
• Acquire note
  • Detect missed notes and retry (maybe drive away and back to let human player drop us a note)
• Drive to speaker
  • This state will prime the speaker and then detect if we are in a location that we can legally shoot from and switch to the shoot state.
• Score note
  • Continue driving toward the speaker while commanding the scoring subsystem to shoot when ready.
  • Once the note has been shot, we will return to driving to the source.

The state machine can be seen in the diagram below: