diff --git a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java index 202a936..06eb75a 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java @@ -80,8 +80,10 @@ public AprilTagPhotonCamera(String name, Transform3d robotToCam, PhotonCameraCon public Optional getEstimatedGlobalPose( Pose2d prevEstimatedRobotPose) { PhotonPipelineResult result = photonCamera.getLatestResult(); - // result.targets.removeIf((target) -> target.getPoseAmbiguity() > 0.2); - // result.targets.removeIf((target) -> target.getFiducialId() > 8); + // result.targets.removeIf((target) -> target.getPoseAmbiguity() > 0.2);w + result.targets.removeIf((target) -> target.getBestCameraToTarget().getTranslation().getNorm() > 7); + // result.targets.removeIf((target) -> + // target.getBestCameraToTarget().getTranslation().getZ() > 1); targetCount = result.targets.size(); photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); @@ -92,7 +94,7 @@ public Optional getEstimatedGlobalPose( robotToCam); } else { detectedAprilTags = new Pose3d[0]; - estimatedRobotPose = new Pose3d(-100, -100, 100, new Rotation3d()); + estimatedRobotPose = new Pose3d(-100, -100, -100, new Rotation3d()); } if (photonEstimatedRobotPose.isPresent()) @@ -128,13 +130,14 @@ public Matrix getEstimationStdDevs(Pose2d estimatedPose, Matrix return estStdDevs; avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) - estStdDevs = multiTagStdDevs; - // Increase std devs based on (average) distance - if (avgDist > 4 && numTags == 1) + if (avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else - estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 5)); + else { + if (numTags > 1) + estStdDevs = multiTagStdDevs.times(1 + (avgDist * avgDist / 5)); + else + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 5)); + } return estStdDevs; } diff --git a/src/main/java/com/techhounds/houndutil/houndlib/subsystems/BaseShooter.java b/src/main/java/com/techhounds/houndutil/houndlib/subsystems/BaseShooter.java index 49c75ea..2cef0e8 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/subsystems/BaseShooter.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/subsystems/BaseShooter.java @@ -9,8 +9,6 @@ public interface BaseShooter { void setVoltage(double voltage); - public Command spinAtCurrentVelocityCommand(); - public Command spinAtVelocityCommand(Supplier goalVelocitySupplier); public Command setOverridenSpeedCommand(Supplier speed); diff --git a/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java b/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java index cae1004..b0ca427 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java @@ -182,7 +182,7 @@ public double getDriveMotorPosition() { @Override public double getDriveMotorVelocity() { - return driveMotor.getVelocity().getValue(); + return driveMotor.getVelocity().getValue() * SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE; } @Override diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index e334459..3c74146 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.1", + "version": "2024.1.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.1" + "version": "2024.1.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.1", + "version": "2024.1.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 4107440..69a4079 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-1", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-1" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-1", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-1", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,