From 83db16794f0aab84892c69db5f743256a4ce40c5 Mon Sep 17 00:00:00 2001 From: Team 2648 Date: Fri, 4 Apr 2025 09:22:33 -0400 Subject: [PATCH] changes to vision filtering and more logging --- .../pathplanner/paths/30 Right to HP.path | 4 +-- .../pathplanner/paths/HP to 330 Right.path | 8 +++--- .../deploy/pathplanner/paths/L Backup.path | 8 +++--- .../java/frc/robot/subsystems/Drivetrain.java | 27 +++++++++++++++---- 4 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index 9d44a71..ccfa902 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.649657534252619, - "y": 6.474186643842743 + "x": 4.843032786885245, + "y": 6.30266393442623 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Right.path b/src/main/deploy/pathplanner/paths/HP to 330 Right.path index 71bb973..6b76674 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Right.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Right.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.65625, - "y": 5.05594262295082 + "x": 3.5723360655737704, + "y": 4.996004098360656 }, "prevControl": { - "x": 3.3407106164383564, - "y": 5.581841595553559 + "x": 3.256796682012127, + "y": 5.521903070963395 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index fb45c41..4d0efd8 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.65625, - "y": 5.05594262295082 + "x": 3.5723360655737704, + "y": 4.996004098360656 }, "prevControl": null, "nextControl": { - "x": 3.523750264999205, - "y": 5.267942198952092 + "x": 3.4398363305729753, + "y": 5.208003674361928 }, "isLocked": false, "linkedName": "L" diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 3cc0d61..deca0d1 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -112,7 +112,7 @@ public class Drivetrain extends SubsystemBase { }, new Pose2d(), VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)), - VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)) + VecBuilder.fill(1, 1, Units.degreesToRadians(360)) ); pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints); @@ -177,11 +177,17 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.10, 0.10, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ - if(vision.getOrangeDist() < 60){ + if(vision.getOrangeDist() < 60 && getVelocity() < 2){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); + }else if(vision.getOrangeDist() < 100 && getVelocity() < 2){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); + }else if(vision.getOrangeDist() < 60 && getVelocity() > 2){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); + }else if(vision.getOrangeDist() < 100 && getVelocity() > 2){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -195,11 +201,19 @@ public class Drivetrain extends SubsystemBase { } } Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); + Logger.recordOutput("orange dist", vision.getOrangeDist()); + Logger.recordOutput("orange detected", vision.getOrangeTagDetected()); if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ - if(vision.getBlackDist() < 60){ + if(vision.getBlackDist() < 60 && getVelocity() < 2){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); + }else if(vision.getBlackDist() < 100 && getVelocity() < 2){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); + }else if(vision.getBlackDist() < 60 && getVelocity() > 2){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); + }else if(vision.getBlackDist() < 100 && getVelocity() > 2){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ @@ -212,7 +226,10 @@ public class Drivetrain extends SubsystemBase { } } Logger.recordOutput("black pose", new Pose3d(blackPose2d)); - + Logger.recordOutput("black dist", vision.getBlackDist()); + Logger.recordOutput("orange detected", vision.getBlackTagDetected()); + + Logger.recordOutput("drive velocity", getVelocity()); Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition())); if(musicTimer.get()>10){