Add even better autos for VAFAL

[aidnem]

Purpose The purpose of this addition is to make better, faster, and higher scoring autos for Falls Church.

Auto Options

• Pure 6 note: S3-W3-W2-W1-C1-C2 This seemed to already be implemented. At the moment, this auto is over the time limit (16.47s when it needs to be 15). Either we must hyper-optimize the paths or just wait for faster autos to be tested so that we can just make the robot move faster.

• 2 notes from centerline: S3-C5-C4 Should work and be well within the time limit at around 11.20 seconds. However, it has yet to be tested on the actual robot.

• 3 notes from centerline: S3-C5-C4-C3 This auto is over the time limit, sitting at around 16.82 seconds. It could either be optimized by moving the shoot point further away from the speaker, or by using a faster set of constraints for the robot. However, both of these solutions require us to use the physical robot to test. Therefore, it is waiting on either:

  1. Test shooting from slightly outside the wing (shooting from the bottom side of the field is the same distance as a slightly over half-court shot on the side of the field that we have built).
  2. Test faster autos (if the robot can move faster, it should be able to make the auto without much difficulty).

• Any 4 notes off the floor from any start position Not really sure about this one yet, and it is not implemented as of now.

[jkleiber]

@redPlover we should load all our auto paths in the CommandSwerveDrivetrain constructor rather than lazy loading a path at auto init. We can maintain a HashMap of PathPlannerAutos based on the auto chooser keys and then send the relevant command with getAutoCommand at auto init. This will help with our delay problems because file loading takes a non-trivial length of time.

Also they just released a path planner update that has some warmup commands we can run in disabled mode. Let's try pulling those in as well