Since the inner goal is behind the target, I’ve been trying to figure out how to accurately get the distance to it, especially when the robot is shooting from the side. The Limelight can estimate the distance to the center of the retroreflective target, and this works perfectly fine when the robot is centered with the target because you can just add an offset, but when shooting from the side, using this process becomes more and more inaccurate. Here is a diagram for more clarification:

I suppose the actual distance to the inner goal could be calculated using the Law of Cosines, but I don’t know how to find the angle phi or any other angles that are part of the triangle for that matter. The limelight documentation lists the network table entry ts (skew), but I am not sure if this corresponds to any of the angles in the triangle. Perhaps the better solution would be to simply use the SolvePnP 3D features and take the magnitude of the (x, z) vector wrt the inner goal point centered at the origin, but this does not solve the aiming problem. I am not sure of how to aim towards the inner goal and not towards the center of the target using the 3D features. Does aiming towards the inner goal have to do with any of the pitch, yaw, or roll components of the 3D pose reported by the PnP? If it does, will I have to compensate for the fact that the Limelight is mounted at an angle? I cannot use the tx value to aim because that will give the horizontal angle to the center of the target and not to the center of the inner goal. I could offset the tx angle by the angle theta, but I do not know how to find that either. There are probably many possible solutions to this problem, so I am curious to see how you all have solved it (if at all). Any help would be greatly appreciated. Thank you!

So if you know the robots current angle based off the gyro, and you know the goal is always at the same angle relative to the field you can do some vector math to figure out the x,y position of the inner goal using estimates from the camera. If the camera gives you a distance to the goal and the gyro + vision angle gives you the angle to the target and the goal is always at a constant x,y relative to the field you can use that info to just do a vector add to the outer goal target and then do a atan from the inner goal delta to get the angle to aim at.

We used code like this in both 2020 and 2021 to increase our inner goal percentage. Unfortunately a realistic angle to aim for the inner port is around 10 deg off center ether direction from the center line (This is account for shooting through the smaller lower section of the hexagon, the drawing is not close to scale). Most shooters/turrets can only hold about +/- 2-5 degrees of shot accuracy in practice, so this usable zone becomes even smaller. At this point your really better off having your drivers line up center.

That being said, Phi is found using your robots gyro, you need to assume the robot started in a known orientation. If your limelight is mounted at an angle you technically should be accounting for the tilt (PnP does this by design, but needs to be done using a 3D rotation if your taking the trigonometric approach) but in practice this tends to pretty negligible considering your turret/shooter will be on center while shooting, minimizing this effect. Once you have found your (x,y), wpilib has some useful classes (translation2D and rotation2d) that will let you solve for theta, and then offset your tx.

You know the distance to the outer target, your angle to that point, and you know the inner target is a fixed distance straight back of the outer target. From this you can use simple geometry (side-angle-side) and trig functionsvto compute the other side length and angle that you want.

For the “fun” of it, I worked an example. In this example you are 20’ down field and 2’ from the wall (in your trench). So if your limelight tells you 252" at 19.5°, then the back target would be 280.5" at 14.9° NB: I was lazy and did this in the horizontal plane. For greater accuracy you would want to do this “in 3-D”. Especially at closer distances where the z-axis would become significant. If you did the calculations in the plane defined by the three points (shooter exit center point x,y,z), outer port center point (x,y,z) and inner point (x,y,z) it would still be a 2-D calculation. But I don’t know how high your shooter is off the floor.

The easy way to get φ is from the aspect ratio of the MBR (minimum bounding rectangle) of the retro-reflective goal tape. To a first approximation, his aspect ratio (width/height) will be proportional to cosφ.