Vision smoothing

[mvog2501]

When we snap to a cube position, the drive jitters all around as it updates with slightly different poses from PhotonVision. One possible solution could be to add a rolling average, another could be to only sample so often or only when we aren't snapping.

[rmheuer]

Probably only jitters for cube positions since the camera could not see the AprilTag when at cone nodes. The camera can now see them always so this is more of an issue