# How to use vision to navigate to hatch panel target perpendicularly using Limelight?

We started using the limelight for vision this year but our programming team is struggling to find a way to use the data the limelight gives us to approach a target perpendicularly if we start facing it at an angle. Does anyone have the logic for this figured out? Any sample code would be great as well if available. Thanks!

1 Like

My team, #4028 the Beak Squad, is using readings for the NavX to calculate what we call â€śAngle 2â€ť, which is the current angle relative to the plane of that target (in your case you want â€śangle 2â€ť to be zero, meaning perpendicular).
Assuming that our robot will always start in the same orientation, every vision target on the field has a desired heading when approaching. For the left rocket front face (the one parallel to the field side wall), the heading is -90, for the sides of the left rocket, either -61.25 or -151.25â€¦ etc. Given the target that we want to go for, the math is just (Target heading - given Limelight angle â€śtxâ€ť (horizontal theta from center of target to center of screen) - robot heading from Limelight (assuming initial heading is 0)). This equation given â€śangle 2â€ť relative to the plane of the target; Idealy, this angle is 0, sygnifying that the robot is perpendicular to the plane. Granted, the equation will spit out the wrong value if the target your Limelight sees is not the intended target, but otherwise it works perfect. Also, my team just uses the NavX.getYaw() function to get the heading, but any Gyro would work as long as it is properly mounted. Hope this helps!!

Our solution for finding the angle we are relative to the hatch target is through the ratio of the bounding box horizontal vs vertical. From the front, the ratio should be at a maximum, and from the side, it should be close to 0. Because youâ€™re using the ratio, the distance shouldnâ€™t matter for determining the angle.

Essentially thereâ€™s a cosine relationship between the angle and the ratio, so we can use
originalRatio*cos(angle) = perspectiveRatio

``````NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry thor = table.getEntry("thor");
NetworkTableEntry tvert = table.getEntry("tvert");
double currentRatio = thor.getDouble(0.0) / tvert.getDouble(0.0);
double originalRatio = 77.0 / 35.0;  // the largest possible ratio from the front

double ratio = Math.min(1, currentRatio/originalRatio); // finding acos of a value > 1 will give NaN

return Math.toDegrees(Math.acos(ratio));``````
2 Likes

Ah yes! Iâ€™ve tried doing something similar to this, but using both targets separately instead of both targets together. Iâ€™ll have to try this when I get the chance.