[CreativeName55]

That is a really nice looking vision system. One suggestion though, if I may, may be to look into properties, as it looks like you're using SmartDashboard? Properties, such as IntegerProperties and DoubleProperties, allow you to easily change the constants being used without redeploying code. Found out about them, and they are super useful for fixing little quirks in things like vision