gyro constants handled

diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
index 5492f69df25cdb0189645a4ea8bd7294449991c0..9419a8c3c1b8249c818a08ede79ee0410ee20825 100644 (file)
--- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java
+++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
@@ -141,6 +141,9 @@ public class DriveConstants {
         /* Neutral Modes */
         public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
         public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
+
+        /* Gyro mount pose roll in deg (180.0 if placed under the robot) */
+        public static double GYRO_MOUNT_POSE_ROLL = 0.0; 
     
         /* Drive Motor PID Values */
         public static final double[] P_VALUES = {
@@ -209,10 +212,13 @@ public class DriveConstants {
                 // MK5n 
                 INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
 
-
+                // Gear ratios 
                 DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0);
                 STEER_GEAR_RATIO = 287.0 / 11.0;
 
+                // Gyro is mounted under the robot 
+                GYRO_MOUNT_POSE_ROLL = 180.0; 
+
                 MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
 
             } else if(robotId == RobotId.WaffleHouse){

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
index 1520fdf0d1b32911cca15d4d0074d03c569e11f8..1c20db33d304970af74c6a1e996f5e2d55350a09 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
@@ -41,9 +41,11 @@ public class GyroIOPigeon2 implements GyroIO {
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;
   private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
+  private Pigeon2Configuration config = new Pigeon2Configuration(); 
 
   public GyroIOPigeon2() {
-    pigeon.getConfigurator().apply(new Pigeon2Configuration());
+    config.MountPose.MountPoseRoll = DriveConstants.GYRO_MOUNT_POSE_ROLL; 
+    pigeon.getConfigurator().apply(config);
     pigeon.getConfigurator().setYaw(0.0);
     yaw.setUpdateFrequency(250);
     yawVelocity.setUpdateFrequency(50.0);