Merge branch 'gyro_trim'

Author: markkassab

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -73,6 +73,9 @@ public class Drive extends SubsystemBase {
     static final double ODOMETRY_FREQUENCY = new CANBus(CompTunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0;
     public final double DRIVE_BASE_RADIUS;
 
+    // Gyro degrees-per-rotation correction/trim
+    static final double GYRO_YAW_DEG_PER_ROT_CORRECTION = -0.97;
+
     // These constants should change for every drivebase
     private final LinearVelocity SPEED_12_VOLTS;
     private final RobotConfig PP_CONFIG;

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -19,6 +19,7 @@
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.GyroTrimConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 
@@ -41,14 +42,18 @@ public GyroIOPigeon2(int Pigeon2Id, CANBus bus) {
     pigeon = new Pigeon2(Pigeon2Id, bus);
     yaw = pigeon.getYaw();
     yawVelocity = pigeon.getAngularVelocityZWorld();
-
     pigeon.getConfigurator().apply(new Pigeon2Configuration());
     pigeon.getConfigurator().setYaw(0.0);
     yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY);
     yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
     yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
     yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw());
+
+    // Improve pigeon's yaw accuracy using the Gyroscope Sensitivity Calibration procedure
+    GyroTrimConfigs trimConfigs = new GyroTrimConfigs();
+    trimConfigs.GyroScalarZ = Drive.GYRO_YAW_DEG_PER_ROT_CORRECTION;
+    pigeon.getConfigurator().apply(trimConfigs);
   }
 
   @Override