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

Fix: interpolate avage vision

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-10 17:13:27 -06:00
parent da976c13b9
commit e029739cee
1 changed files with 50 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Vision.java
+49 -29
View File
@@ -102,6 +102,7 @@ public class Vision extends Subsystem {
photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
// resetRotations();
} }
@Override @Override
@@ -111,11 +112,13 @@ public class Vision extends Subsystem {
// cameras[0]. // cameras[0].
} }
public int rotations = 0; // public int[] rotations;
// public Instant[] lastUpdateTimes;
public void resetRotations(){ // public void resetRotations(){
rotations = 0; // rotations = new int[cameras.length];
} // lastUpdateTimes = new Instant[cameras.length];
// }
private Instant lastVisionTime = null; private Instant lastVisionTime = null;
@@ -124,14 +127,17 @@ public class Vision extends Subsystem {
isTagProcessed = false; isTagProcessed = false;
isTagDetected = false; isTagDetected = false;
Instant now = Instant.now();
int cams = 0; int cams = 0;
double X = 0; // double X = 0;
double Y = 0; // double Y = 0;
double Yaw = 0; // double Yaw = 0;
double latency = 0; double latency = 0;
Pose2d pose = null;
for(int i = 0; i < cameras.length; i++){ for(int i = 0; i < cameras.length; i++){
PhotonCamera camera = cameras[i]; PhotonCamera camera = cameras[i];
PhotonPoseEstimator estimator = estimators[i]; PhotonPoseEstimator estimator = estimators[i];
@@ -158,11 +164,18 @@ public class Vision extends Subsystem {
if(estimatedRobotPose.isEmpty()) if(estimatedRobotPose.isEmpty())
continue; continue;
Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d(); if(pose == null)
X += pose.getX(); pose = estimatedRobotPose.get().estimatedPose.toPose2d();
Y += pose.getY(); else
Yaw += pose.getRotation().getDegrees(); pose = pose.interpolate(pose, 0.5);
cams++; // X += pose.getX();
// Y += pose.getY();
// if(X > 6)
// Yaw += (pose.getRotation().getDegrees() + 180) % 360;
// cams++;
isTagProcessed = true; isTagProcessed = true;
@@ -170,31 +183,38 @@ public class Vision extends Subsystem {
lastLatency = latency / cams; lastLatency = latency / cams;
if(isTagProcessed || true){ if(isTagProcessed){
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
// Pose2d e = new Pose2d();
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;
}
}
// lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360));
lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians()); // // double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise
SmartDashboard.putNumber("Rotations", rotations);
// 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;
// }
// }
lastVisionPose = pose;
// lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
// lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
// SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
// SmartDashboard.putNumber("Rotations", rotations);
lastVisionTime = now; lastVisionTime = now;
} }