Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to wpilib 2023 beta 7 (#607)

Browse Source 
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
shueja-personal
2022-12-16 17:05:23 -08:00
committed by GitHub
parent da1aabae3a
commit bb63af601d
198 changed files with 6339 additions and 4525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
View File

@@ -26,9 +26,10 @@ package org.photonvision;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -166,6 +167,9 @@ public class SimVisionSystem {
tgtList.forEach(
(tgt) -> {
var camToTargetTrans = new Transform3d(cameraPose, tgt.targetPose);
// Generate a transformation from camera to target,
// ignoring rotation.
var t = camToTargetTrans.getTranslation();
// Rough approximation of the alternate solution, which is (so far) always incorrect.
@@ -181,15 +185,21 @@ public class SimVisionSystem {
double area_px = tgt.tgtAreaMeters2 / getM2PerPx(distMeters);
double yawDegrees = Units.radiansToDegrees(Math.atan2(t.getY(), t.getX()));
var translationAlongGround =
new Translation2d(
tgt.targetPose.toPose2d().getX() - cameraPose.toPose2d().getX(),
tgt.targetPose.toPose2d().getY() - cameraPose.toPose2d().getY());
var camAngle = cameraPose.getRotation().toRotation2d();
var camToTgtRotation =
new Rotation2d(translationAlongGround.getX(), translationAlongGround.getY());
double yawDegrees = camToTgtRotation.minus(camAngle).getDegrees();
double camHeightAboveGround = cameraPose.getZ();
double tgtHeightAboveGround = tgt.targetPose.getZ();
double camPitchDegrees = Units.radiansToDegrees(cameraPose.getRotation().getY());
var transformAlongGround =
new Transform2d(cameraPose.toPose2d(), tgt.targetPose.toPose2d());
double distAlongGround = transformAlongGround.getTranslation().getNorm();
double distAlongGround = translationAlongGround.getNorm();
double pitchDegrees =
Units.radiansToDegrees(