Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

17
wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
View File

@@ -11,9 +11,11 @@ namespace frc {
units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
units::meter_t targetHeight,
units::radian_t cameraPitch,
units::radian_t targetPitch) {
units::radian_t targetPitch,
units::radian_t targetYaw) {
return (targetHeight - cameraHeight) /
units::math::tan(cameraPitch + targetPitch);
(units::math::tan(cameraPitch + targetPitch) *
units::math::cos(targetYaw));
}
frc::Pose2d EstimateFieldToRobot(
@@ -22,11 +24,12 @@ frc::Pose2d EstimateFieldToRobot(
const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) {
return EstimateFieldToRobot(
EstimateCameraToTarget(frc::Translation2d{CalculateDistanceToTarget(
cameraHeight, targetHeight,
cameraPitch, targetPitch),
targetYaw},
fieldToTarget, gyroAngle),
EstimateCameraToTarget(
frc::Translation2d{
CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch,
targetPitch, targetYaw.Radians()),
targetYaw},
fieldToTarget, gyroAngle),
fieldToTarget, cameraToRobot);
}