allow for dynamic std devs for pose estimation

HOC-Team-3534 · Feb 29, 2024 · 129aef9 · 129aef9
1 parent 3285471
commit 129aef9
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 3 deletions.
diff --git a/3534CTRESwerve.json b/3534CTRESwerve.json
@@ -1,7 +1,7 @@
 {
   "fileName": "3534CTRESwerve.json",
   "name": "3534CTRESwerve",
-  "version": "2024.2.2",
+  "version": "2024.2.3",
   "frcYear": 2024,
   "uuid": "1fcedb64-1708-4258-9f56-fc21dc0d7c78",
   "mavenUrls": [
@@ -12,7 +12,7 @@
     {
       "groupId": "com.github.HOC-Team-3534",
       "artifactId": "3534CTRESwerve",
-      "version": "2024.2.2"
+      "version": "2024.2.3"
     }
   ],
   "jniDependencies": [],

diff --git a/src/main/java/swerve/CommandSwerveDrivetrain.java b/src/main/java/swerve/CommandSwerveDrivetrain.java
@@ -18,10 +18,14 @@
 import com.pathplanner.lib.path.PathPlannerTrajectory;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -46,6 +50,15 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
     public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency,
             double maxSpeed, HolonomicPathFollowerConfig holoConfig,
             SwerveModuleConstants... modules) {
+        this(driveTrainConstants, OdometryUpdateFrequency,
+                VecBuilder.fill(0.1, 0.1, 0.1),
+                VecBuilder.fill(0.9, 0.9, 0.9), maxSpeed, holoConfig, modules);
+    }
+
+    public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency,
+            Matrix<N3, N1> odometryStandardDeviation, Matrix<N3, N1> visionStandardDeviation,
+            double maxSpeed, HolonomicPathFollowerConfig holoConfig,
+            SwerveModuleConstants... modules) {
         super(driveTrainConstants, OdometryUpdateFrequency, modules);
         this.maxSpeed = maxSpeed;
         pathplanner.configureHolonomic(() -> this.getState().Pose, this::seedFieldRelative,

diff --git a/src/main/java/swerve/SwerveDrivetrain.java b/src/main/java/swerve/SwerveDrivetrain.java
@@ -3,8 +3,12 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -17,7 +21,18 @@ public enum RobotCentricity {
 
     public SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency,
             SwerveModuleConstants[] modules) {
-        super(driveTrainConstants, OdometryUpdateFrequency, modules);
+        this(driveTrainConstants,
+                OdometryUpdateFrequency,
+                VecBuilder.fill(0.1, 0.1, 0.1),
+                VecBuilder.fill(0.9, 0.9, 0.9),
+                modules);
+    }
+
+    public SwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency,
+            Matrix<N3, N1> odometryStandardDeviation, Matrix<N3, N1> visionStandardDeviation,
+            SwerveModuleConstants[] modules) {
+        super(driveTrainConstants, OdometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation,
+                modules);
         centricityChooser.setDefaultOption("Field Centric", RobotCentricity.FieldCentric.toString());
         centricityChooser.addOption("Robot Centric", RobotCentricity.RobotCentric.toString());
         SmartDashboard.putData("Centricity Chooser", centricityChooser);