Working

Vision not tested
Estimated Pids
auto not even working

Author: PikeRoboDevils1018

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

@@ -21,7 +21,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final Mode simMode = Mode.SIM;
+  public static final Mode simMode = Mode.REAL;
   public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
 
   public static enum Mode {

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

@@ -151,7 +151,7 @@ private void configureButtonBindings() {
             drive,
             () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
-            () -> -controller.getRightX()));
+            () -> controller.getRightX()));
 
     // Lock to 0° when A button is held
     controller

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

@@ -25,11 +25,9 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -45,7 +43,6 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
-import frc.robot.Robot;
 import frc.robot.Constants.Mode;
 import frc.robot.util.LocalADStarAK;
 import java.util.concurrent.locks.Lock;
@@ -74,8 +71,6 @@ public class Drive extends SubsystemBase {
   private SwerveDrivePoseEstimator poseEstimator =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
 
-    
-
   public Drive(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
@@ -127,7 +122,7 @@ public Drive(
             new SysIdRoutine.Mechanism(
                 (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
 
-      // poseEstimator.resetPose(DriveConstants.startingPose);
+    // poseEstimator.resetPose(DriveConstants.startingPose);
   }
 
   @Override

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

@@ -19,7 +19,6 @@
 
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.RobotConfig;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -64,7 +63,7 @@ public class DriveConstants {
   public static final int backRightTurnCanId = 9;
 
   // Drive motor configuration
-  public static final int driveMotorCurrentLimit = 50;
+  public static final int driveMotorCurrentLimit = 40;
   public static final double wheelRadiusMeters = Units.inchesToMeters(2);
   public static final double driveMotorReduction = 6.75; // SDS Mk4i L2
   public static final DCMotor driveGearbox = DCMotor.getNEO(1);
@@ -97,7 +96,7 @@ public class DriveConstants {
   public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec
 
   // Turn PID configuration
-  public static final double turnKp = 0.01;
+  public static final double turnKp = 0.8;
   public static final double turnKd = 0;
   public static final double turnSimP = 0.01;
   public static final double turnSimD = 0.0;
@@ -142,5 +141,5 @@ public class DriveConstants {
           .withTrackLengthTrackWidth(Inches.of(24), Inches.of(24))
           // Configures the bumper size (dimensions of the robot bumper)
           .withBumperSize(Inches.of(30), Inches.of(30));
-public static Pose2d startingPose = new Pose2d(3,3,new Rotation2d());
+  public static Pose2d startingPose = new Pose2d(3, 3, new Rotation2d());
 }

src/main/java/frc/robot/subsystems/drive/GyroIOSim.java

@@ -32,7 +32,7 @@ public GyroIOSim(GyroSimulation gyroSimulation) {
 
   @Override
   public void updateInputs(GyroIOInputs inputs) {
-    inputs.connected = true; 
+    inputs.connected = true;
     inputs.yawPosition = gyroSimulation.getGyroReading();
     inputs.yawVelocityRadPerSec = gyroSimulation.getMeasuredAngularVelocity().in(RadiansPerSecond);