Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01: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

24
wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
View File

@@ -19,10 +19,10 @@ class ComputerVisionUtilTest {
var targetHeight = 3;
var camPitch = Units.degreesToRadians(0);
var targetPitch = Units.degreesToRadians(30);
var targetYaw = Units.degreesToRadians(0);
var dist =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch);
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
Assertions.assertEquals(3.464, dist, 0.01);
@@ -33,7 +33,7 @@ class ComputerVisionUtilTest {
dist =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch);
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
Assertions.assertEquals(5.671, dist, 0.01);
camHeight = 3;
@@ -43,9 +43,21 @@ class ComputerVisionUtilTest {
dist =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch);
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
Assertions.assertEquals(3.464, dist, 0.01);
camHeight = 1;
targetHeight = 3;
camPitch = Units.degreesToRadians(0);
targetPitch = Units.degreesToRadians(30);
targetYaw = Units.degreesToRadians(30);
dist =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
Assertions.assertEquals(4, dist, 0.01);
}
@Test
@@ -64,7 +76,7 @@ class ComputerVisionUtilTest {
ComputerVisionUtil.estimateCameraToTarget(
new Translation2d(
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch),
camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()),
targetYaw),
fieldToTarget,
gyroAngle),
@@ -82,7 +94,7 @@ class ComputerVisionUtilTest {
ComputerVisionUtil.estimateCameraToTarget(
new Translation2d(
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch),
camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()),
targetYaw),
fieldToTarget,
gyroAngle),