Merge branch 'main' into phoenix-pro

diff --cc src/main/java/frc/robot/constants/swerve/DriveConstants.java
Simple merge

diff --cc src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
Simple merge

diff --cc src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
index 1cd53941c543f625701c87bddb5252c5d4196373,dd63dcdd35b6d9043c96b57be6693d2fc60b6a9e..53c36d5f620520650b3ab67f4756d418358fd8d2
--- 1/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
--- 2/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java
@@@ -32,6 -30,10 +32,13 @@@ public interface GyroIO 
  
    public default void updateInputs(GyroIOInputs inputs) {}
  
 +  /** returns the yaw status signal for time-synced odometry. */
 +  public default StatusSignal<?> getYawSignal() { return null; }
++
+   /**
+    * set the yaw angle of the gyro.
+    *
+    * @param rotation the new yaw angle
+    */
+   public default void setYaw(Rotation2d rotation) {}
  }

diff --cc src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
index 5b1d9e3b67c5c9cc4d2de6bcbcd6cfa18ff6ba2d,c6211f59e9d43b916e1a3f3c77470171e169ef6a..1e083616a04854088ee8188781b2287d99975136
--- 1/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
--- 2/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java
@@@ -72,8 -72,8 +72,13 @@@ public class GyroIOPigeon2 implements G
      yawPositionQueue.clear();
    }
  
 +  @Override
 +  public StatusSignal<Angle> getYawSignal() {
 +    return yaw;
 +  }
++  
+   @Override
+   public void setYaw(Rotation2d rotation) {
+     pigeon.getConfigurator().setYaw(rotation.getDegrees());
+   }
  }

diff --cc src/main/java/frc/robot/subsystems/drivetrain/Module.java
index bfdf7556a57a5e0589be4c995f721fc17d984e48,49910f1e1d7538d3495848f75e37e92285658fd4..9c26df3f9ed605e9a00a044a973f7ce3acde348c
--- 1/src/main/java/frc/robot/subsystems/drivetrain/Module.java
--- 2/src/main/java/frc/robot/subsystems/drivetrain/Module.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
@@@ -249,10 -250,12 +252,12 @@@ public class Module implements ModuleIO
          }
          if (isOpenLoop) {
              double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
 -            driveMotor.set(percentOutput);
 +            driveMotor.setControl(new DutyCycleOut(percentOutput).withEnableFOC(true));
          } else {
              double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
-             Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+             if (!Constants.DISABLE_LOGGING) {
+                 Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
+             }
              
              double feedforward = velocity * moduleConstants.getDriveV();
              driveMotor.setControl(
@@@ -439,22 -450,4 +466,22 @@@
          return inputs.odometryTimestamps;
      }
  
- }
 +    /** returns the drive position status signal for time-synced odometry. */
 +    public StatusSignal<Angle> getDrivePositionSignal() {
 +        return drivePosition;
 +    }
 +
 +    /** returns the turn position status signal for time-synced odometry. */
 +    public StatusSignal<Angle> getTurnPositionSignal() {
 +        return turnPosition;
 +    }
 +
 +    /** returns the turn absolute position status signal for time-synced odometry. */
 +    public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
 +        return turnAbsolutePosition;
 +    }
 +
 +    public TalonFX[] getMotors() {
 +        return new TalonFX[]{angleMotor, driveMotor};
 +    }
+ }