Fix 2024 auto align

src/main/java/frc4388/robot/Constants.java

@@ -99,50 +99,50 @@ public final class Constants {
         public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
         public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
 
-        // public static final double FORWARD_OFFSET = 270; // 0, 90, 180, 270
+        public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
-        public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
+        // public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
 
         public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10);
 
         private static final class ModuleSpecificConstants {
             //Front Left
-            // private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
+            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
-            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
+            // private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
             private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
-            // private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
+            private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
-            private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
+            // private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
             private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
             
             //Front Right
-            // private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
+            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
-            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
+            // private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
             private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
             private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
-            // private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+            private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
-            private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
+            // private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
 
             //Back Left
-            // private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
+            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
-            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
+            // private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
             private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
             private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
-            // private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+            private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
-            private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
+            // private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
             
             //Back Right
-            // private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
+            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
-            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
+            // private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
             private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
-            // private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
+            private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
-            private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
+            // private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
             private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
         }
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -89,6 +89,7 @@ public class RobotContainer {
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
+        
         configureButtonBindings();        
         configureVirtualButtonBindings();
         new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
@@ -101,7 +102,7 @@ public class RobotContainer {
         m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
             m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
                                             getDeadbandedDriverController().getRight(),
-                                false);
+                                true);
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -79,7 +79,7 @@ public class SwerveDrive extends Subsystem {
     
     if (fieldRelative) {
       swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
-        .withVelocityX(leftStick.getX()*speedAdjust)
+        .withVelocityX(leftStick.getX()*-speedAdjust)
         .withVelocityY(leftStick.getY()*speedAdjust)
         .withRotationalRate(rightStick.getX()*rotSpeedAdjust)
       );

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -110,7 +110,7 @@ public class Vision extends Subsystem {
         }
 
         lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
-        lastVisionPose.rotateBy(Rotation2d.k180deg);
+        // lastVisionPose.rotateBy(Rotation2d.k180deg);
         // lastVisionPose = new Pose2d(
         //     lastVisionPose.getTranslation(),
         //     lastPhysOdomPose.getRotation()