Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

2024x2025

Browse Source
This commit is contained in:
C4llSiqn
2025-01-15 16:32:13 -07:00
parent 036514a478
commit 5f9a38f55d
4 changed files with 21 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Vision.java
+5 -1
View File
@@ -3,6 +3,7 @@ package frc4388.robot.subsystems;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N1;
@@ -73,6 +74,9 @@ public class Vision extends Subsystem {
@Override
public void periodic() {
var result = camera.getLatestResult();
// var results = camera.getAllUnreadResults();
// if (results.size() == 0) return;
// var result = results.get(results.size()-1);
isTag = result.hasTargets();
// Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
@@ -106,7 +110,7 @@ public class Vision extends Subsystem {
}
lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
// lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation()));
lastVisionPose.rotateBy(Rotation2d.k180deg);
// lastVisionPose = new Pose2d(
// lastVisionPose.getTranslation(),
// lastPhysOdomPose.getRotation()