Create DragonVision class

[witcpalek]

This class should be a subclass of State.

Class will call DragonLimelight during its Run method, so get the Limelights in the constructor.

Run Method

• Determine if there is a vision targets or not
• If there are multiple targets visible, utilize them to get a better answer
• If there is, determine the confidence (AprilTags probably have a higher confidence, multiple targets increases the confidence, closer targets should have more weight than farther targets, the closer the alignment (straightahead) the better the confidence, etc.)
• If there is a reasonable confidence, then call the chassis to update the odometry

Should have extra methods for:

• bool AlignedWithCubeNode() const;
• bool AlignedWithConeNode() const;
• bool AlignedWithSubstation() const;
• units::distance::inch_t DistanceFromCubeNode() const;
• units::distance::inch_t DistanceFromConeNode() const;
• units::distance::inch_t DistanceFromSubstation() const;
• units::angle::degrees_t AngleFromCubeNode() const;
• units::angle::degrees_t AngleFromConeNode() const;
• units::angle::degrees_t AngleFromSubstation() const;
• bool AlignedWithCubeGamePiece() const;
• bool AlignedWithConeGamePiece() const;
• units::distance::inch_t DistanceFromCubeGamePiece() const;
• units::distance::inch_t DistanceFromConeGamePiece() const;
• units::angle::degrees_t AngleFromCubeGamePiece() const;
• units::angle::degrees_t AngleFromConeGamePiece() const;
• frc::Pose2d GetRobotPosition() const;

For first pass, implement the structure (No-Op methods).

Then create new issues for each of the related items.

[witcpalek]

@ImAPenguin56 @muscatd75

Let's refactor this a bit .... I think RobotState should implement a publish/subscribe mechanism (see issue 149) so, DragonVision can subscribe to events that cause it to change pipelines and then it can use the IsAligned(), GetDistance(), GetAngle() and GetPose methods and it will work on the current pipelines.

Front limelight will react to retro, april tag, cone and cube change Back limelight will only have april tag for the moment When indicating we went off the ground, this may switch the front camera to april tag if the current odometry thinks it will give better results (otherwise back limelight can be used).

[witcpalek]

@ImAPenguin56

We should look at https://www.chiefdelphi.com/t/introducing-limelight-lib/425660 as it might give us a cleaner interface