Adds stateful command for Automated Teleop

[aidnem]

Purpose
This PR adds a new command to control teleop autonomously.
Closes #182.

Features
A state machine with the following states:

• START
  • Initial state when the command is initialized.
  • If the robot has a note, switch to DRIVE_TO_SPEAKER.
  • If the robot doesn't have a note, switch to DRIVE_TO_SOURCE.
• DRIVE_TO_SOURCE
  • Once a note is detected with vision (not yet implemented) or the robot reaches the source, switch to ACQUIRE_NOTE to go intake the note.
• ACQUIRE_NOTE
  • Currently just sits still and spins the intake, waiting for a note to be pushed into the robot
  • Once the robot has a note, switch to DRIVE_TO_SPEAKER
• DRIVE_TO_SPEAKER
  • This state will prime the speaker and then detect if the robot is in a location that it can safely shoot from and switch to SHOOT_NOTE. This currently only checks range to the speaker in deciding whether or not to switch to the score note state.
• SHOOT_NOTE
  • Continue driving toward the speaker while commanding the scoring subsystem to shoot when ready.
  • Once the note has been shot, return to DRIVE_TO_SOURCE.

The state machine can be seen in the diagram below:

Outstanding Work

• Add note vision to DRIVE_TO_SOURCE to detect when a note is near and switch to ACQUIRE_NOTE.
• Revamp ACQUIRE_NOTE to use note vision and/or retry logic to be able to actually intake notes by itself.
• Finalize PathPlanner paths to sources (especially shop source)
  • Consider adding a better way to switch between shop and competition environments (currently decided by just commenting/uncommenting various lines throughout the project). This might be better as a separate issue (maybe in conjunction with the JSON configs issue), and it can probably wait until then.

[aidnem]

The Mermaid diagram for the state machine is complete. It can be found under doc/AutomatedTeleop.md. The table this diagram was generated from can be found in the controls Google Drive or in CSV format under doc/AutomatedTeleop.csv.

[aidnem]

@linglejack06 I've noticed that the method to follow a path in the drivetrain disables alignment. Is this strictly necessary, or could we keep alignment enabled and drive to the path anyway? I'd prefer to prime and align as soon as the robot starts driving back from the source, but if it can't auto align, it would have to stop and wait before it could shoot upon arrival.

[linglejack06]

@aidnem by using the path following it defaults to their alignment, but Im pretty sure there is something in the path configuration where i can override rotation ( we could then keep the align state on ). Ill look into it tonight or tomorrow morning and add it to this PR it shouldnt take long

[linglejack06]

@aidnem I have just added the rotation target override which should work. We can test on Wednesday.

[jkleiber]

Can you test in the sim 👀

[aidnem]

I'll be out of the house all day but I can probably try it out tonight or tomorrow afternoon. Depending on whether or not they allow me into shop on Monday we could test it in real life then as well.

[linglejack06]

I can try in sim if I have time. I also just remembered I forgot to remove the align state disabled in drive to path method so I will update that next time I get on my computer

[aidnem]

Coaches have informed me that I am not welcome in shop and therefore testing will have to wait until next Monday (no school Wednesday) when they allow everybody back in. I'll test sim when I get the chance.

[linglejack06]

just pusehd the update with removal of the align state manual. If we keep structure how it is now, then best choice would be to set align state and target in the auto teleop command.

If we add a state machine to drive class to handle automated driving, then i could add the align state and target in that state machine. @jkleiber do you have a preference? I know you had mentioned adding the automated state machine to the drivetrain class

[jkleiber]

@linglejack06 if possible, using the Action + State framework we've been using is preferred. But if there's no reason to do Actions (i.e. any state can transition to any other state) then you can just make a mode machine where you set the drive mode in the command.

For example, you could have modes for: auto align, path follow, manual, etc

[aidnem]

@jkleiber Here's the first test of running it in the sim. Sorry for the terrible quality, GitHub won't let me upload a video over 10MB so I had to shrink and trim it severely.

automated.teleop.1st.sim.test.cropped.mov

This shows that the state machine is at least somewhat functional (it drives where it is trying to and then transitions to 'ACQUIRE_NOTE'). Once I figure out how to use note simulation, I'll test a full cycle.
Ignoring the extreme sketchiness of how driving looks in the sim, I think it looks promising.
Also for some reason the path is on the wrong side of the field at the moment, I'll fix that as well (this was driving to shopSource, not aiming at the actual source.

[jkleiber]

@aidnem that video looks good - if you or @linglejack06 want to take a crack at #170 it would obviously be nice to not have this crusty driving behavior in the sim

[aidnem]

@jkleiber Yeah, the crusty sim behavior is unpleasant. Should we do something like #103 to configure constants with JSON instead? Depending on which is a higher priority I'm fine working on either.

[jkleiber]

@aidnem #186 would actually be a better first step. #103 is for JSON configurable controllers, which is useful but doesn't fix anything besides that

[aidnem]

Oh you're right, I thought they were the same PR.