Drive to apriltag with PhotonVision

[ErikCald]

The idea here is to use PhotonVision running on a raspberry pi using the global shutter black and white camera. With this setup, it can reach a very high framerate, at a very high resolution and with practically no motion blur.

This issue is going to be about using this vision data to drive the robot, but will require setting up PhotonVision (its really easy).

All of this can be incorporated into a command, but later it might make sense to refactor some of it into a VisionSubsystem (such as the rolling average filter).

Incremental develpoment is important, recommended steps:

• [x] 1. Decide the best way to calculate distance with photonvision and create it.
• [x] 2. Run 2 seperate pid loops using distance and yaw to move the robot until it is facing the target and at the correct distance.
• [x] 3. Use distance and yaw to calculate X and Y, then add X and Y to odometry values. This is now our desired setpoint, alongside the desired heading for the given target. Then run 3 pid controllers which use odometry as the measurement and this new desired setpoint (issue #8 is supposed to do this for you if it is finished by the time you are ready for it) . This should be far more resilient to intermittent vision data.
• [x] 4. The desired setpoint should be fairly stable and a rolling average filter or something similar could be used to get a more accurate estimated of the target. The filter should be reset everything the driver starts to align with a new target.
• [x] 5. Improvements can be made to better integrate this driver aid with the driver. Instead of the driver pressing a button and the vision instantly starts aligning with a target, the driver should hold down a button but maintain control of the robot. With the button held down, vision will start scanning for the target and if enough data is found, vision takes control and aligns to the apriltag. This transition of control from driver to vision will be far more seamless. It also avoids the issue where the driver presses the button but no vision data is present so the robot just sits there.

[ErikCald]

Notes so I don't forget them:

• [ ] There are syntax errors due to the VSCode troubles
• [ ] hasData() in PhotonSubsystem should probably use >= instead of == because the variable called data counts beyond maxData (also petition to rename data and maxData to sampleSize and maxSampleSize)
• [ ] can use reset(-1) in constructor of PhotonSubsystem to improve readability
• [ ] yaw method needs a return statement
• [ ] Need a command (preferably a command factory method in PhotonSubsystem) to just reset the PhotonSubsystem to a tag. PathPlanner can call this ahead of using a PhotonMoveToTarget (skipping WaitForData)
• [ ] Here is a way to turn the WaitForVisionCommand into a command factory method in PhotonSubsystem instead:

  public Command getWaitForVisionCommand(int tagId) {
      return new FunctionalCommand(
          () -> resetMethodHere(tagId), // reset tagId on command initialize
          () -> {}, // do nothing on command execute
          (interrupted) -> {}, // do nothing on command end
          () -> checkMethodHere(), // Check if command should finish (command isFinished)
          this); // Use this subsystem as the single requirement (PhotonSubsystem)
  }

[ErikCald]

Pitch distance Calc: https://docs.wpilib.org/en/stable/docs/software/vision-processing/introduction/identifying-and-processing-the-targets.html#distance