12/11 End of Day - Tuning

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

@@ -183,6 +183,8 @@ public void periodic() {
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM);
+
+    Logger.recordOutput("Odometry/Robot", getPose());
   }
 
   /**
@@ -290,7 +292,7 @@ public double getFFCharacterizationVelocity() {
   }
 
   /** Returns the current odometry pose. */
-  @AutoLogOutput(key = "Odometry/Robot")
+  // @AutoLogOutput(key = "Odometry/Robot")
   public Pose2d getPose() {
     return poseEstimator.getEstimatedPosition();
   }

src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java

@@ -60,15 +60,22 @@ public class DriveConstants {
       Underrotated → pointing toward the robot’s left (your right)
   */
   public static final Rotation2d frontLeftZeroRotation =
-      new Rotation2d(-0.7853181997882291).minus(new Rotation2d(Degrees.of(16 + 5)));
+      new Rotation2d(-0.7853181997882291)
+          .minus(new Rotation2d(Degrees.of(16 + 5)))
+          .minus(Rotation2d.kCW_90deg);
   public static final Rotation2d frontRightZeroRotation =
-      new Rotation2d(-1.9785268942462368).minus(new Rotation2d(Degrees.of(-7 - 3)));
+      new Rotation2d(-1.9785268942462368)
+          .minus(new Rotation2d(Degrees.of(-7 - 3)))
+          .minus(Rotation2d.kCW_90deg);
   public static final Rotation2d backLeftZeroRotation =
       new Rotation2d(1.2279650529278354)
           .minus(new Rotation2d(Degrees.of(-8)))
-          .minus(Rotation2d.kCCW_90deg);
+          .minus(Rotation2d.kCCW_90deg)
+          .minus(Rotation2d.kCW_90deg);
   public static final Rotation2d backRightZeroRotation =
-      new Rotation2d(-0.8600462118731901).minus(new Rotation2d(Degrees.of(17.5 + 2)));
+      new Rotation2d(-0.8600462118731901)
+          .minus(new Rotation2d(Degrees.of(17.5 + 2)))
+          .minus(Rotation2d.kCW_90deg);
 
   // Device CAN IDs
   public static final int pigeonCanId = 13;
@@ -90,7 +97,7 @@ public class DriveConstants {
 
   // Drive motor configuration
   public static final int driveMotorCurrentLimit = 50;
-  public static final double wheelRadiusMeters = Units.inchesToMeters(2);
+  public static final double wheelRadiusMeters = Units.inchesToMeters(1.981);
   public static final double driveMotorReduction = 5.14; // SDS mk4 with L4 gearing
   public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1);
 
@@ -101,10 +108,11 @@ public class DriveConstants {
       (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec
 
   // Drive PID configuration
+  //
   public static final double driveKp = 0.0;
   public static final double driveKd = 0.0;
-  public static final double driveKs = 0.0;
-  public static final double driveKv = 0.1;
+  public static final double driveKs = 0.11605;
+  public static final double driveKv = 0.11034;
   public static final double driveSimP = 0.05;
   public static final double driveSimD = 0.0;
   public static final double driveSimKs = 0.0;

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

@@ -18,6 +18,7 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MountPoseConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -35,7 +36,15 @@ public class GyroIOPigeon2 implements GyroIO {
   private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
 
   public GyroIOPigeon2() {
-    pigeon.getConfigurator().apply(new Pigeon2Configuration());
+    pigeon
+        .getConfigurator()
+        .apply(
+            new Pigeon2Configuration()
+                .withMountPose(
+                    new MountPoseConfigs()
+                        .withMountPosePitch(-0.8275826573371887)
+                        .withMountPoseRoll(-2.518435001373291)
+                        .withMountPoseYaw(1.0797953605651855)));
     pigeon.getConfigurator().setYaw(0.0);
     yaw.setUpdateFrequency(odometryFrequency);
     yawVelocity.setUpdateFrequency(50.0);

src/main/java/frc/robot/generic/util/SwerveBuilder.java

@@ -25,7 +25,8 @@ public class SwerveBuilder {
    * @param driveController the controller to bind buttons to, traditionally in port 0
    * @return Drive subsystem, for further integration
    */
-  public static Drive buildDefaultDrive(CommandXboxController driveController) {
+  public static Drive buildDefaultDrive(
+      CommandXboxController driveController, double speedMultiplier) {
     Drive drive;
     switch (Constants.currentMode) {
       case REAL:
@@ -64,9 +65,9 @@ public static Drive buildDefaultDrive(CommandXboxController driveController) {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
             drive,
-            () -> -driveController.getLeftY(),
-            () -> -driveController.getLeftX(),
-            () -> driveController.getRightX()));
+            () -> driveController.getLeftY() * speedMultiplier,
+            () -> driveController.getLeftX() * speedMultiplier,
+            () -> driveController.getRightX() * speedMultiplier));
     driveController
         .y()
         .onTrue(
@@ -78,4 +79,8 @@ public static Drive buildDefaultDrive(CommandXboxController driveController) {
                 .ignoringDisable(true));
     return drive;
   }
+
+  public static Drive buildDefaultDrive(CommandXboxController driveController) {
+    return buildDefaultDrive(driveController, 1);
+  }
 }

src/main/java/frc/robot/outReach/RobotContainer.java

@@ -51,7 +51,7 @@ public class RobotContainer implements AbstractRobotContainer {
   private final CANBus canBus = new CANBus();
 
   // Subsystems
-  private final Drive drive = SwerveBuilder.buildDefaultDrive(controller);
+  private final Drive drive = SwerveBuilder.buildDefaultDrive(controller, 0.75);
   private final Turret turret;
 
   // Dashboard inputs