SwerveSim Fixed

Author: eitanVIP

src/main/java/frc/robot/Constants/FieldConstants.java

@@ -64,14 +64,11 @@ public class FieldConstants {
     public static AprilTagFieldLayout getFieldLayout(List<Integer> ignoredTags) {
         AprilTagFieldLayout layout;
 
-        if (RobotState.getInstance().isSimulated()) layout = kUseOurField ? kOurFieldLayout : kBlueFieldLayout;
-        else {
-            if (kUseOurField) layout = kOurFieldLayout;
-            else
-                layout = RobotState.getInstance().getAlliance() == DriverStation.Alliance.Blue
-                    ? kBlueFieldLayout
-                    : kRedFieldLayout;
-        }
+        if (kUseOurField) layout = kOurFieldLayout;
+        else
+            layout = RobotState.getInstance().getAlliance() == DriverStation.Alliance.Blue
+                ? kBlueFieldLayout
+                : kRedFieldLayout;
 
         if (!ignoredTags.isEmpty()) layout.getTags().removeIf(tag -> ignoredTags.contains(tag.ID));
 

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

@@ -161,7 +161,8 @@ public static final class Mod3 {
 
     public static class Simulation {
         public static final double kSimToRealSpeedConversion = 0.02; // meters per 0.02s -> meters per 1s
-        public static final double kAcceleration = 20;
+        public static final double kAcceleration = 12;
+        public static final double k0Acceleration = 18;
     }
 
     public static final class AutoConstants {

src/main/java/frc/robot/Swerve/SwerveIO.java

@@ -150,10 +150,6 @@ public double lookAtTarget(Pose2d target, boolean invert, Rotation2d sheer) {
 
 		lookAtTranslation = lookAtTranslation.rotateBy(sheer);
 
-		lookAtTranslation = RobotState.getInstance().isSimulated()
-				? new Translation2d(lookAtTranslation.getX(), -lookAtTranslation.getY())
-				: lookAtTranslation;
-
 		return lookAt(invert ? lookAtTranslation.rotateBy(Rotation2d.fromDegrees(180)) : lookAtTranslation, 1);
 	}
 
@@ -377,7 +373,7 @@ public void periodic() {
 								_demand.driverInput.vxMetersPerSecond,
 								_demand.driverInput.vyMetersPerSecond,
 								lookAtTarget(
-										_demand.targetPose, /*RobotState.getInstance().getRobotState() == RobotStates.NOTE_SEARCH*/
+										_demand.targetPose,
 										false,
 										SwerveConstants.kShootingAngleError.unaryMinus())),
 						SwerveConstants.kFieldRelative);

src/main/java/frc/robot/Swerve/SwerveSimulated.java

@@ -11,6 +11,7 @@ public class SwerveSimulated extends SwerveIO {
 	private ChassisSpeeds _currentChassisSpeeds = new ChassisSpeeds();
 	private final SlewRateLimiter _xAcceleration = new SlewRateLimiter(SwerveConstants.Simulation.kAcceleration);
 	private final SlewRateLimiter _yAcceleration = new SlewRateLimiter(SwerveConstants.Simulation.kAcceleration);
+	private final SlewRateLimiter _0Acceleration = new SlewRateLimiter(SwerveConstants.Simulation.k0Acceleration);
 
 	@Override
 	public void drive(ChassisSpeeds drive, boolean fieldRelative) {
@@ -26,7 +27,7 @@ public void drive(ChassisSpeeds drive, boolean fieldRelative) {
 								* SwerveConstants.Simulation.kSimToRealSpeedConversion,
 				RobotState.getInstance().getRobotPose()
 						.getRotation()
-						.minus(Rotation2d.fromRadians(_currentChassisSpeeds.omegaRadiansPerSecond
+						.plus(Rotation2d.fromRadians(_0Acceleration.calculate(_currentChassisSpeeds.omegaRadiansPerSecond)
 								* SwerveConstants.Simulation.kSimToRealSpeedConversion))));
 	}
 

src/main/java/frc/robot/TeleopCommandBuilder.java

@@ -23,8 +23,7 @@ public static Command swerveDrive(
 				() -> {
 					double lx = -MathUtil.applyDeadband(translation.get().getX(), SwerveConstants.kJoystickDeadband);
 					double ly = -MathUtil.applyDeadband(translation.get().getY(), SwerveConstants.kJoystickDeadband);
-					double rx = (RobotState.getInstance().isSimulated() ? 1 : -1)
-							* MathUtil.applyDeadband(rotation.get().getX(), SwerveConstants.kJoystickDeadband);
+					double rx = -MathUtil.applyDeadband(rotation.get().getX(), SwerveConstants.kJoystickDeadband);
 					double ry = -MathUtil.applyDeadband(rotation.get().getY(), SwerveConstants.kJoystickDeadband);
 
 					double finalRotation =