Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Fix vision noise?

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-10 13:13:48 -06:00
parent a264e466bc
commit 0458654281
1 changed files with 19 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Vision.java
+19 -22
View File
@@ -111,20 +111,12 @@ public class Vision extends Subsystem {
// cameras[0]. // cameras[0].
} }
public double rotations = 0; public int rotations = 0;
public void resetRotations(){ public void resetRotations(){
rotations = 0; rotations = 0;
} }
public void addRotation(){
rotations += 180;
}
public void subtractRotation(){
rotations -= 180;
}
private Instant lastVisionTime = null; private Instant lastVisionTime = null;
@@ -178,27 +170,32 @@ public class Vision extends Subsystem {
lastLatency = latency / cams; lastLatency = latency / cams;
if(isTagProcessed){ if(isTagProcessed || true){
Instant now = Instant.now(); Instant now = Instant.now();
double curAngle = Yaw/cams; // double curAngle = (Yaw/cams);
double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise
// if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){
// double lastAngle = lastVisionPose.getRotation().getDegrees();
// if(lastAngle - curAngle >= 80){ if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){
// addRotation(); double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360;
// }else if(lastAngle - curAngle <= -80){
// subtractRotation(); if(diff > 180){
// } rotations -= 1;
// } }else if(diff < -180){
rotations += 1;
}
}
// SmartDashboard.putNumber("curAngle", curAngle);
// SmartDashboard.putNumber("Rotations", rotations);
lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360));
lastVisionPose = new Pose2d(0, 0, Rotation2d.fromDegrees(curAngle + rotations*360));
SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians());
SmartDashboard.putNumber("Rotations", rotations);
lastVisionTime = now; lastVisionTime = now;
} }
} }