fix(swerve); Adjust module distances.

src/main/java/frc/robot/swerve/DriveCommand.java

@@ -1,14 +1,11 @@
 package frc.robot.swerve;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.lib.SnapRotation;
 import frc.robot.RobotConstants;
 import frc.robot.odometry.Odometry;
 
@@ -26,42 +23,21 @@ public class DriveCommand extends Command {
   private ChassisSpeeds previousChassisSpeeds;
 
   /* Current and previous requests from the driver controller. */
-  private DriveRequest request, previousRequest;
-
-  /* Heading feedback controller. */
-  private final PIDController headingFeedback;
-
-  /** Heading snapper. */
-  private final SnapRotation headingSnapper;
-
-  /** Heading goal. */
-  private State headingGoal, headingSetpoint;
+  private DriveRequest request;
 
   public DriveCommand(CommandXboxController driverController) {
     swerve = Swerve.getInstance();
     odometry = Odometry.getInstance();
 
     addRequirements(swerve);
 
-    headingFeedback = new PIDController(6.0, 0.0, 0.1);
-    headingFeedback.enableContinuousInput(-0.5, 0.5);
-
-    headingGoal = new State(0.0, 0.0);
-    headingSetpoint = new State(0.0, 0.0);
-
     previousChassisSpeeds = new ChassisSpeeds();
 
     this.driverController = driverController;
-    previousRequest = DriveRequest.fromController(driverController);
-
-    headingSnapper = SnapRotation.to(Rotation2d.fromDegrees(90));
-
-    odometry.onYawUpdate(newPose -> resetHeadingGoal());
   }
 
   @Override
   public void initialize() {
-    resetHeadingGoal();
   }
 
   @Override
@@ -73,23 +49,9 @@ public void execute() {
 
     Rotation2d driverRelativeHeading = odometry.getDriverRelativeHeading();
 
-    // if (DriveRequest.startedDrifting(previousRequest, request)) {
-    //   resetHeadingGoal();
-    // }
-
-    // if (request.isSnapping()) {
-    //   setPositionHeadingGoal(headingSnapper.snap(request.driverHeading()));
-    // }
-
     Rotation2d omega = new Rotation2d();
 
-    if (request.isSpinning()) {
-      updateVelocity(request.omega());
-
-      omega = request.omega();
-    } else {
-      // omega = calculateHeadingProfileOmega();
-    }
+    omega = request.omega();
 
     ChassisSpeeds chassisSpeeds =
         clampChassisSpeeds(
@@ -102,8 +64,6 @@ public void execute() {
     swerve.setChassisSpeeds(chassisSpeeds);
 
     previousChassisSpeeds = chassisSpeeds;
-
-    previousRequest = request;
   }
 
   @Override
@@ -146,76 +106,4 @@ private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) {
     return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
   }
 
-  /**
-   * Returns the reference heading to use with the heading motion profile.
-   *
-   * @return the reference heading to use with the heading motion profile.
-   */
-  private Rotation2d getReferenceHeading() {
-    return odometry.getDriverRelativeHeading();
-  }
-
-  /**
-   * Resets the heading goal.
-   *
-   * <p>The robot's orientation becomes the new goal and the motion profile is reset by setting the
-   * setpoint to the goal.
-   */
-  private void resetHeadingGoal() {
-    setPositionHeadingGoal(getReferenceHeading());
-
-    headingSetpoint = headingGoal;
-  }
-
-  /**
-   * Sets the position goal.
-   *
-   * @param goal the position goal.
-   */
-  private void setPositionHeadingGoal(Rotation2d goal) {
-    State state = new State(goal.getRotations(), 0.0);
-
-    headingGoal = wrapState(state, getReferenceHeading().getRotations());
-  }
-
-  /**
-   * Updates the motion profile with the robot's rotational velocity.
-   *
-   * @param omega the robot's rotational velocity.
-   */
-  private void updateVelocity(Rotation2d omega) {
-    Rotation2d heading = getReferenceHeading();
-
-    State state = new State(heading.getRotations(), omega.getRotations());
-
-    headingGoal = wrapState(state, heading.getRotations());
-  }
-
-  /**
-   * Calculates the robot's rotational velocity.
-   *
-   * @return the robot's calculated rotational velocity.
-   */
-  private Rotation2d calculateHeadingProfileOmega() {
-    headingSetpoint =
-        SwerveConstants.ROTATION_MOTION_PROFILE.calculate(
-            RobotConstants.PERIODIC_DURATION, headingSetpoint, headingGoal);
-
-    double omegaRotationsPerSecond =
-        headingFeedback.calculate(getReferenceHeading().getRotations(), headingSetpoint.position);
-
-    return Rotation2d.fromRotations(omegaRotationsPerSecond);
-  }
-
-  /**
-   * Wraps an input state to be within the range of a measured position.
-   *
-   * @param state the input state.
-   * @param measurement the measured position.
-   * @return a new state within the range of the measured position.
-   */
-  private State wrapState(State state, double measurement) {
-    double minError = MathUtil.inputModulus(state.position - measurement, -0.5, 0.5);
-    return new State(minError + measurement, state.velocity);
-  }
 }

src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -54,10 +54,10 @@ public static class MK4iConstants {
   public static final boolean USE_PHOENIX_PRO_FOC = false;
 
   /** Module X offset in meters. */
-  public static final double X_OFFSET = Units.inchesToMeters(11.375);
+  public static final double X_OFFSET = Units.inchesToMeters(10.375);
 
   /** Module Y offset in meters. */
-  public static final double Y_OFFSET = Units.inchesToMeters(11.375);
+  public static final double Y_OFFSET = Units.inchesToMeters(10.375);
 
   /** Swerve's CAN bus. */
   public static final String SWERVE_BUS = "swerve";