Programming
Examples
Vision
LimelightOS
Distance Estimation

Behind the Scenes: Distance Estimation

Distance Estimation

If your vision tracking camera is mounted on your robot such that the angle between the ground plane and its line of sight does not change, then you can use this technique to very accurately calculate the distance to a target. You can then use this distance value to either drive your robot forward and back to get into the perfect range or adjust the power of a launching mechanism.

See the below diagram. In this context, all of the variables are known: the height of the target (h2) is known because it is a property of the field. The height of your camera above the floor (h1) is known and its mounting angle is known (a1). The Limelight (or your vision system) can tell you the y angle to the target (a2).

Sample Code for Distance Estimation

    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
    NetworkTableEntry ty = table.getEntry("ty");
    double targetOffsetAngle_Vertical = ty.getDouble(0.0);
 
    // how many degrees back is your limelight rotated from perfectly vertical?
    double limelightMountAngleDegrees = 25.0;
 
    // distance from the center of the Limelight lens to the floor
    double limelightLensHeightInches = 20.0;
 
    // distance from the target to the floor
    double goalHeightInches = 60.0;
 
    double angleToGoalDegrees = limelightMountAngleDegrees + targetOffsetAngle_Vertical;
    double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
 
    //calculate distance
    double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) / Math.tan(angleToGoalRadians);

Explanation

The above code calculates the distance from the Limelight to the target. The Limelight is mounted at a known angle to the ground, and the target is at a known height above the ground. The Limelight can measure the angle to the target, and the code calculates the distance to the target using trigonometry.

How to Use

  1. Mount your camera at a known angle to the ground.
  2. Measure the height of the camera lens above the ground.
  3. Measure the height of the target above the ground.
  4. Use the Limelight to measure the angle to the target.
  5. Use the code above to calculate the distance to the target.

Additional Notes

  • The code above assumes that the target is directly in front of the camera. If the target is off to the side, you will need to adjust the calculations accordingly.
  • The code above assumes that the target is at a known height above the ground. If the target is at an unknown height, you will need to use a different method to calculate the distance.

Conclusion

Distance estimation is a powerful tool that can be used to improve the accuracy of your robot. By knowing the distance to a target, you can adjust your robot's position or power output to achieve the desired result. This can be especially useful in tasks such as shooting a ball into a goal or picking up an object from the ground. By using the Limelight to measure the angle to the target and the height of the camera above the ground, you can calculate the distance to the target and use this information to improve the performance of your robot.

References

TelemetryYear 2023-24 SPYDER