Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

changes to vision filtering and more logging

Browse Source
This commit is contained in:
Team 2648 2025-04-04 09:22:33 -04:00
parent 3dcbac25cc
commit 83db16794f
4 changed files with 32 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/deploy/pathplanner/paths/30 Right to HP.path
View File

@ -8,8 +8,8 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 5.649657534252619, "x": 4.843032786885245,
"y": 6.474186643842743 "y": 6.30266393442623
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null

8
src/main/deploy/pathplanner/paths/HP to 330 Right.path
View File

@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 3.65625, "x": 3.5723360655737704,
"y": 5.05594262295082 "y": 4.996004098360656
}, },
"prevControl": { "prevControl": {
"x": 3.3407106164383564, "x": 3.256796682012127,
"y": 5.581841595553559 "y": 5.521903070963395
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,

8
src/main/deploy/pathplanner/paths/L Backup.path
View File

@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.65625, "x": 3.5723360655737704,
"y": 5.05594262295082 "y": 4.996004098360656
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.523750264999205, "x": 3.4398363305729753,
"y": 5.267942198952092 "y": 5.208003674361928
}, },
"isLocked": false, "isLocked": false,
"linkedName": "L" "linkedName": "L"

27
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -112,7 +112,7 @@ public class Drivetrain extends SubsystemBase {
}, },
new Pose2d(), new Pose2d(),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)), 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); 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()); 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.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))); 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 // 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 pose", new Pose3d(orangePose2d));
Logger.recordOutput("orange dist", vision.getOrangeDist());
Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ 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))); 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()){ 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 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())); Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>10){ if(musicTimer.get()>10){