Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Improve accuracy of ComputerVisionUtil.calculateDistanceToTarget() (#4215)

Browse Source
This commit is contained in:
chen perach
2022-05-06 18:36:58 +03:00
committed by GitHub
parent b1abf455c1
commit ef7ed21a9d
5 changed files with 56 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
View File

@@ -32,15 +32,17 @@ public final class ComputerVisionUtil {
* Positive values up.
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
* values up.
* @param targetYawRadians The yaw of the target in the camera's lens in radians.
* @return The estimated distance to the target in meters.
*/
public static double calculateDistanceToTarget(
double cameraHeightMeters,
double targetHeightMeters,
double cameraPitchRadians,
double targetPitchRadians) {
double targetPitchRadians,
double targetYawRadians) {
return (targetHeightMeters - cameraHeightMeters)
/ Math.tan(cameraPitchRadians + targetPitchRadians);
/ (Math.tan(cameraPitchRadians + targetPitchRadians) * Math.cos(targetYawRadians));
}
/**
@@ -76,7 +78,11 @@ public final class ComputerVisionUtil {
estimateCameraToTarget(
new Translation2d(
calculateDistanceToTarget(
cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians),
cameraHeightMeters,
targetHeightMeters,
cameraPitchRadians,
targetPitchRadians,
targetYaw.getRadians()),
targetYaw),
fieldToTarget,
gyroAngle),