Adds stateful command for Automated Teleop

[aidnem]

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:

[aidnem]

After further thought, we shouldn't need a prime and score state because the scoring subsystem will be in a shooting state as long as we are on our side of the field (just like in auto).

[jkleiber]

@aidnem if possible it would be good to prime while driving from the source to the wing just to reduce cycle time

[aidnem]

@jkleiber That's what I had in mind. My plan is just to have it tell scoring to shoot when ready as soon as it has a line of sight on the speaker and then just keep driving until it shoots.

[aidnem]

@jkleiber Can we consider removing the separation between the DRIVE_TO_SOURCE and the INTAKE_NOTE states, and just having DRIVE_TO_SOURCE run intake while driving to the source? This way, we can eliminate a state and some transition logic. When we need to retry an intake, the RETRY_INTAKE state can just drive away from the intake before giving control back to DRIVE_TO_SOURCE. DRIVE_TO_SOURCE can be renamed to something else that's more descriptive if necessary.

[linglejack06]

@aidnem would that not be unnecessary to have to drive back and try again instead of just having the retry intake run intake logic again?

[aidnem]

@linglejack06 Here's my thinking: I feel that it would be simpler to have DRIVE_TO_SOURCE just swap to RETRY_INTAKE after a certain amount of time has passed. Then, RETRY_INTAKE drives away and then switches back to DRIVE_TO_SOURCE.

If we make RETRY_INTAKE both drive away and then drive back and try to intake, it will have to keep track of what point it's at in that process separately from the state machine, because the state will remain at RETRY_INTAKE. Therefore, we'll need extra logic to tell it when to drive away, how long to wait, and then when to drive back, all within the case for RETRY_INTAKE. To avoid this, we could break up RETRY_INTAKE into multiple states, but then the 2nd state would be a clone of DRIVE_TO_SOURCE anyway, so there's no point.

If you're noticing something that I haven't considered yet I'm completely open to changing it, but that's my justification for right now as to why I have started to work on it this way.

[jkleiber]

Can someone draw the block diagram for this state machine / link the state transition matrix with a description of what each state does?

I think doing so would help make it clear what the solution is

[aidnem]

@jkleiber apologies for the scuffed diagram, I didn't have access to my computer so I drew it by hand instead:

I'll type it all up and append it to the matrix Google Sheet tomorrow when I have time.

[jkleiber]

What if we did something like this:

There is value in having states outside of the driving states. As you've identified, picking up the note at the source won't be trivial without some auto targeting (which we can ignore for now as we build up to that feature being available). I'd recommend just allowing that to be a state itself that gates when you go back to the speaker. This also allows you to abandon going all the way to the source if we happen to find a note along the path.

As we get more advanced, we could even add a state for checking if there are notes in the wing to avoid wasting time, but that's future work

[aidnem]

@jkleiber I see. If I understand correctly, the only modification this would make to the current system would be a separate state for shooting the note (and not removing the intake note state). Would this be necessary, or could we just let the scoring subsystem decide when it's in range?

[jkleiber]

@aidnem yeah you could implement it that way. I separated it out because I want the shooter subsystem to be able to shoot from the wing line if manually commanded to do so, but want auto teleop to shoot from a closer distance at first. You could simply parameterize the range threshold as part of setting the action.

The other change is that there's no "retry intake" state. That can all be handled in one note acquisition state

[aidnem]

@jkleiber That makes sense, I'll update the matrix right now and the code when I get home. In your system, would the note acquisition state just sit and wait to be able to detect a note using vision and then drive and pick it up? We could consider letting it spin a little to see if it can see any notes around it (just as a potential future feature). This way, it wouldn't have to keep track of whether it was driving away to retry.

[jkleiber]

Yeah the "Acquire Note" state would basically sit still until a note is detected and then go get it.

I think the driving aspect can be handled by a state machine in the drive subsystem, and we can keep the command high level (i.e the command only knows that the robot is trying to get a note, it doesn't need to specify how)

We'll incrementally build up to doing more advanced things (crawl-walk-run style). So at first the robot will just sit there with the intake on until someone feeds it a note. Then we can add note detection and driving to get the note. Then we can add searching once those things are solid

[aidnem]

@jkleiber I'm implementing the changes now. What would be the best way to go about determining new field constants for the positions of the source?

[jkleiber]

You could pick the coordinates out of path planner

[aidnem]

The feature is close enough to being finished that I think we can work on knocking out the final features, so I've created a draft PR: #194.