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!
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.