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].
}
public double rotations = 0;
public int rotations = 0;
public void resetRotations(){
rotations = 0;
}
public void addRotation(){
rotations += 180;
}
public void subtractRotation(){
rotations -= 180;
}
private Instant lastVisionTime = null;
@@ -178,27 +170,32 @@ public class Vision extends Subsystem {
lastLatency = latency / cams;
if(isTagProcessed){
if(isTagProcessed || true){
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){
// addRotation();
// }else if(lastAngle - curAngle <= -80){
// subtractRotation();
// }
// }
if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){
double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360;
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;
}
}