Full 3D solving (e.g. SolvePNP) and simple trig (a la retroreflective tape) are both valid methods of calculating the robot pose based on target positions. The benefit of simple trig is that it may produce measurements that are more stable than a 3D solve since it is less sensitive to noise in the corner detections of the tags. However, the downside of this approach is that an inaccurate camera position (to a precision of a small fraction of a degree) will consistently offset every measurement. This is a significant problem for judging distance for shooting, where you might be significantly off from your true position and be unable to tell because the measurements are consistent with each other.
3D tracking (once calibrated) is much less subject to these types of offsets, and noise can be handled with properly tuned filters; this is especially true for our use case with multiple high-resolution and high-framerate cameras. Another major benefit is that data from multiple tags in a single frame can be combined to form a very accurate estimate even from far distances, while simple trig needs to deal with each tag separately (increasing noise). 3D targeting also allows for the gyro to be corrected during the match, though this is admittedly not a major consideration given the accuracy of modern FRC gyros.
There’s nothing wrong with the simple trig approach to localization, but we would much rather calibrate the cameras and tune appropriate filters in order to ensure that we can benefit from multiple tags and have distance measurements that aren’t consistently offset.
WPILib used to have a utility class to help with these calculations, but unfortunately the relevant methods were removed. The method that did this from our 2022 code is here as a reference.
The most critical part of making a pipeline like this work is getting the camera position, height, and angle exactly right. We actually used a fudge factor in 2022 where we adjusted the camera angle for the calculations by ~0.1° in order to get the distances accurate enough for shooting.
We wanted to take advantage of FOC on the Talons, and torque-based control is the native way that FOC works. Torque-based control has a number of other benefits described here, including the fact that a velocity feedforward (kv) is generally unnecessary when tuning. As we described, the only characterization we run is for static friction (we don’t use ka and haven’t found a mechanism where it is needed). While CTRE provides a hybrid method that allows for voltage and duty-cycle based FOC, we would rather use FOC in its native form instead of relying on a barely-documented algorithm that ultimately makes the behavior harder to tune.
This is our first year using FOC and torque-based control, so the other reason for using it is to gain experience with it and understand where it has more or less benefit for our use cases.