From 81720cfae2ab45e2c06bdd83463806588593401f Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Tue, 14 Jan 2025 20:43:08 +0200
Subject: [PATCH 001/158] created SwerveModule.java

---
 .idea/gradle.xml                              |   2 +-
 .../frc/robot/subsystems/SwerveModule.java    | 171 ++++++++++++++++++
 2 files changed, 172 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/subsystems/SwerveModule.java

diff --git a/.idea/gradle.xml b/.idea/gradle.xml
index de9b4b6..f2c1963 100644
--- a/.idea/gradle.xml
+++ b/.idea/gradle.xml
@@ -5,7 +5,7 @@
     <option name="linkedExternalProjectsSettings">
       <GradleProjectSettings>
         <option name="externalProjectPath" value="$PROJECT_DIR$" />
-        <option name="gradleJvm" value="corretto-17" />
+        <option name="gradleJvm" value="17" />
         <option name="modules">
           <set>
             <option value="$PROJECT_DIR$" />
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
new file mode 100644
index 0000000..705d1d4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -0,0 +1,171 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.control.FeedForwardSettings;
+import com.spikes2212.control.PIDSettings;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class SwerveModule extends DashboardedSubsystem {
+
+    public static final double MAX_SPEED = 4;
+    public static final double MIN_SPEED = 0.4;
+    public static final double MAX_TURN = 3;
+    private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
+    private static final double TURN_GEAR_RATIO = 1 / 12.8;
+    private static final double WHEEL_DIAMETER = 4 * 0.0254;
+    private static final double DEGREES_IN_ROTATIONS = 360;
+    private static final double SECONDS_IN_MINUTE = 60;
+    private static final double ABSOLUTE_POSITION = 1;
+
+    private final TalonFX driveMotor;
+    private final SparkMax turnMotor;
+    private final CANcoder absoluteEncoder;
+    private final boolean cancoderInverted;
+    private final boolean driveInverted;
+    private final double offset;
+    private final RelativeEncoder driveEncoder;
+    private final RelativeEncoder turnEncoder;
+    private final PIDSettings drivePIDSettings;
+    private final PIDSettings turnPIDSettings;
+    private final FeedForwardSettings driveFeedForwardSettings;
+    private final FeedForwardSettings turnFeedForwardSettings;
+    private final EncoderConfig driveEncoderConfig;
+    private final EncoderConfig turnEncoderConfig;
+
+    public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CANcoder absoluteEncoder,
+                        boolean cancoderInverted, boolean driveInverted, double offset,
+                        RelativeEncoder driveEncoder, RelativeEncoder turnEncoder,
+                        PIDSettings drivePIDSettings, PIDSettings turnPIDSettings,
+                        FeedForwardSettings driveFeedForwardSettings,
+                        FeedForwardSettings turnFeedForwardSettings) {
+        super(namespace);
+        this.driveMotor = driveMotor;
+        this.turnMotor = turnMotor;
+        this.absoluteEncoder = absoluteEncoder;
+        this.cancoderInverted = cancoderInverted;
+        this.driveInverted = driveInverted;
+        this.offset = offset;
+        this.driveEncoder = driveEncoder;
+        this.turnEncoder = turnEncoder;
+        this.drivePIDSettings = drivePIDSettings;
+        this.turnPIDSettings = turnPIDSettings;
+        this.driveFeedForwardSettings = driveFeedForwardSettings;
+        this.turnFeedForwardSettings = turnFeedForwardSettings;
+        turnEncoderConfig = new EncoderConfig();
+        driveEncoderConfig = new EncoderConfig();
+        createDriveConfig();
+        createTurnController();
+    }
+
+    private void createDriveConfig() {
+        driveEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI);
+        driveEncoderConfig.velocityConversionFactor((DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI) / SECONDS_IN_MINUTE);
+    }
+
+
+    public void configureDriveController() {
+        Slot0Configs driveConfigs = new Slot0Configs();
+        driveConfigs.kP = drivePIDSettings.getkP();
+        driveConfigs.kI = drivePIDSettings.getkI();
+        driveConfigs.kD = drivePIDSettings.getkD();
+        driveConfigs.kS = driveFeedForwardSettings.getkS();
+        driveConfigs.kV = driveFeedForwardSettings.getkV();
+        driveConfigs.kA = driveFeedForwardSettings.getkA();
+        driveMotor.getConfigurator().apply(driveConfigs);
+        driveEncoderConfig.inverted(driveInverted);
+    }
+
+    private void createTurnController() {
+        turnEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
+    }
+
+    public void configureTurnController() {
+        ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
+        turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());
+        turnEncoderConfig.inverted(cancoderInverted);
+    }
+
+    public void configureAbsoluteEncoder() {
+        MagnetSensorConfigs magnetConfigs = new MagnetSensorConfigs()
+                .withAbsoluteSensorDiscontinuityPoint(ABSOLUTE_POSITION)
+                .withSensorDirection(cancoderInverted ? SensorDirectionValue.Clockwise_Positive :
+                        SensorDirectionValue.CounterClockwise_Positive).withMagnetOffset(offset);
+        absoluteEncoder.getConfigurator().apply(magnetConfigs);
+    }
+
+    private void setSpeed(double speed, boolean usePID) {
+        if (usePID) {
+            configureDriveController();
+            driveMotor.setControl(new VelocityDutyCycle(speed));
+        } else driveMotor.set(speed / MAX_SPEED);
+    }
+
+    private void setAngle(double angle) {
+        configureTurnController();
+        turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity);
+    }
+
+    public void stop() {
+        driveMotor.stopMotor();
+        turnMotor.stopMotor();
+    }
+
+    public void set(SwerveModuleState state, boolean usePID) {
+        if (state.speedMetersPerSecond < MIN_SPEED){
+            stop();
+            return;
+        }
+        state = optimize(state, turnEncoder.getPosition());
+        setAngle(state.angle.getDegrees());
+        setSpeed(state.speedMetersPerSecond, usePID);
+    }
+
+    private double normalizeAngleRelativeToEncoder(double currentAngle, double desiredAngle) {
+        int rotations = (int) (currentAngle / DEGREES_IN_ROTATIONS);
+        if (currentAngle < 0) rotations--;
+        desiredAngle += rotations * DEGREES_IN_ROTATIONS;
+        return desiredAngle;
+    }
+
+    private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
+        double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
+        while (Math.abs(desiredAngle - currentAngle) > 90) {
+            if (desiredAngle - currentAngle > 0) {
+                desiredAngle -= 180;
+            }
+            else {
+                desiredAngle += 180;
+            }
+            state.speedMetersPerSecond *= -1;
+        }
+        return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));
+    }
+
+    public double getAbsoluteAngle() {
+        return absoluteEncoder.getAbsolutePosition().getValueAsDouble() * DEGREES_IN_ROTATIONS;
+    }
+
+    public void resetRelativePosition() {
+        turnEncoder.setPosition(getAbsoluteAngle());
+    }
+
+    @Override
+    public void configureDashboard() {
+        namespace.putNumber("absolute angle", this::getAbsoluteAngle);
+        namespace.putNumber("velocity", driveEncoder::getVelocity);
+        namespace.putNumber("relative angle", turnEncoder::getPosition);
+    }
+}

From 5c248cee6a373476063b6be77728036e67865408 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Tue, 14 Jan 2025 21:04:14 +0200
Subject: [PATCH 002/158] created Drivetrain.java

---
 .../java/frc/robot/subsystems/Drivetrain.java | 84 +++++++++++++++++++
 .../frc/robot/subsystems/SwerveModule.java    |  2 +-
 2 files changed, 85 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/subsystems/Drivetrain.java

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
new file mode 100644
index 0000000..2144a6e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -0,0 +1,84 @@
+"package frc.robot.subsystems;
+
+import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.dashboard.Namespace;
+import com.studica.frc.AHRS;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.subsystems.SwerveModule;
+
+public class Drivetrain extends DashboardedSubsystem {
+    private static final double TRACK_WIDTH = 0.61;
+    private static final double TRACK_LENGTH = 0.58;
+
+    public static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
+    public static final Translation2d FRONT_LEFT_WHEEL_POSITION =
+            new Translation2d(TRACK_WIDTH / 2, TRACK_LENGTH / 2);
+    public static final Translation2d FRONT_RIGHT_WHEEL_POSITION =
+            new Translation2d(TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
+    public static final Translation2d BACK_LEFT_WHEEL_POSITION =
+            new Translation2d(-TRACK_WIDTH / 2, TRACK_LENGTH / 2);
+    public static final Translation2d BACK_RIGHT_WHEEL_POSITION =
+            new Translation2d(-TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
+
+    private static final String NAMESPACE_NAME = "drivetrain";
+
+    private final SwerveModule frontLeft;
+    private final SwerveModule frontRight;
+    private final SwerveModule backLeft;
+    private final SwerveModule backRight;
+    private final SwerveDriveKinematics kinematics;
+    private final AHRS gyro;
+
+    public Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
+                      SwerveModule backRight, AHRS gyro) {
+        super(NAMESPACE_NAME);
+        this.frontLeft = frontLeft;
+        this.frontRight = frontRight;
+        this.backLeft = backLeft;
+        this.backRight = backRight;
+        this.kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
+                FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        this.gyro = gyro;
+    }
+
+    public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
+                      boolean usePID) {
+        ChassisSpeeds speeds;
+        if (fieldRelative) {
+            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed,
+                    gyro.getRotation2d());
+        }
+        else {
+            speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
+        }
+        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, CENTER_OF_ROBOT);
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveModule.MAX_SPEED);
+        frontLeft.set(states[0], usePID);
+        frontRight.set(states[1], usePID);
+        backLeft.set(states[2], usePID);
+        backRight.set(states[3], usePID);
+    }
+
+    public void stop() {
+        frontLeft.stop();
+        frontRight.stop();
+        backLeft.stop();
+        backRight.stop();
+    }
+
+    public void resetRelativeEncoder() {
+        frontLeft.resetRelativeEncoder();
+        frontRight.resetRelativeEncoder();
+        backLeft.resetRelativeEncoder();
+        backRight.resetRelativeEncoder();
+    }
+
+
+    @Override
+    public void configureDashboard() {
+
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 705d1d4..c5205d9 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -158,7 +158,7 @@ public double getAbsoluteAngle() {
         return absoluteEncoder.getAbsolutePosition().getValueAsDouble() * DEGREES_IN_ROTATIONS;
     }
 
-    public void resetRelativePosition() {
+    public void resetRelativeEncoder() {
         turnEncoder.setPosition(getAbsoluteAngle());
     }
 

From 0f4944a0ae85db0b2c1a3384c15a6383304b1ecf Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Tue, 14 Jan 2025 21:06:40 +0200
Subject: [PATCH 003/158] created Drivetrain.java

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 2144a6e..86d38cb 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,7 +1,6 @@
-"package frc.robot.subsystems;
+package frc.robot.subsystems;
 
 import com.spikes2212.command.DashboardedSubsystem;
-import com.spikes2212.dashboard.Namespace;
 import com.studica.frc.AHRS;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;

From 6fd98f70419dbe732f260c49c44098e84692df75 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Tue, 14 Jan 2025 21:22:57 +0200
Subject: [PATCH 004/158] created Drive.java

---
 src/main/java/frc/robot/commands/Drive.java | 36 +++++++++++++++++++++
 1 file changed, 36 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/Drive.java

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
new file mode 100644
index 0000000..c5f61fa
--- /dev/null
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Drivetrain;
+
+import java.util.function.Supplier;
+
+public class Drive extends Command {
+    private final Drivetrain drivetrain;
+    private final Supplier<Double> xSpeed;
+    private final Supplier<Double> ySpeed;
+    private final Supplier<Double> rotationsSpeed;
+    private final boolean fieldRelative;
+    private final boolean usePID;
+
+    public Drive( Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
+        this.drivetrain = drivetrain;
+        this.xSpeed = xSpeed;
+        this.ySpeed = ySpeed;
+        this.rotationsSpeed = rotationsSpeed;
+        this.fieldRelative = fieldRelative;
+        this.usePID = usePID;
+    }
+
+    @Override
+    public void execute() {
+        double xSpeed = this.xSpeed.get();
+        double ySpeed = this.ySpeed.get();
+        double rotationSpeed = this.rotationsSpeed.get();
+        drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
+    }
+
+    public void end(boolean interrupted) {
+        drivetrain.stop();
+    }
+}

From a4d20fdd575f07d68c0320ced793e895c9673f9a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Tue, 14 Jan 2025 21:24:57 +0200
Subject: [PATCH 005/158] created SwerveModule.java

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index c5205d9..d5eeb5a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -67,6 +67,7 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         driveEncoderConfig = new EncoderConfig();
         createDriveConfig();
         createTurnController();
+        configureAbsoluteEncoder();
     }
 
     private void createDriveConfig() {

From e534b11f0d6312d253b9a3e21d137cf46d947348 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 17:12:14 +0200
Subject: [PATCH 006/158] created SwerveModuleHolder.java

---
 src/main/java/frc/robot/RobotMap.java         | 15 ++++++++++++
 .../frc/robot/subsystems/SwerveModule.java    | 23 +++++++++++--------
 2 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 6745be9..1633428 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -4,6 +4,21 @@ public class RobotMap {
 
     public interface CAN {
 
+        int FRONT_LEFT_DRIVE_TALON_FX = -1;
+        int FRONT_RIGHT_DRIVE_TALON_FX = -1;
+        int BACK_LEFT_DRIVE_TALON_FX = -1;
+        int BACK_RIGHT_DRIVE_TALON_FX = -1;
+
+        int FRONT_LEFT_TURN_SPARK_MAX = -1;
+        int FRONT_RIGHT_TURN_SPARK_MAX = -1;
+        int BACK_LEFT_TURN_SPARK_MAX = -1;
+        int BACK_RIGHT_TURN_SPARK_MAX = -1;
+
+        int FRONT_LEFT_ABSOLUTE_ENCODER = -1;
+        int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
+        int BACK_LEFT_ABSOLUTE_ENCODER = -1;
+        int BACK_RIGHT_ABSOLUTE_ENCODER = -1;
+
     }
     
     public interface DIO {
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index d5eeb5a..1ab34cd 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -1,10 +1,13 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkBase;
@@ -29,24 +32,23 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double SECONDS_IN_MINUTE = 60;
     private static final double ABSOLUTE_POSITION = 1;
 
+
     private final TalonFX driveMotor;
     private final SparkMax turnMotor;
     private final CANcoder absoluteEncoder;
     private final boolean cancoderInverted;
     private final boolean driveInverted;
     private final double offset;
-    private final RelativeEncoder driveEncoder;
     private final RelativeEncoder turnEncoder;
     private final PIDSettings drivePIDSettings;
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
-    private final EncoderConfig driveEncoderConfig;
     private final EncoderConfig turnEncoderConfig;
+    private final MotorOutputConfigs MotorOutput;
 
     public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CANcoder absoluteEncoder,
                         boolean cancoderInverted, boolean driveInverted, double offset,
-                        RelativeEncoder driveEncoder, RelativeEncoder turnEncoder,
                         PIDSettings drivePIDSettings, PIDSettings turnPIDSettings,
                         FeedForwardSettings driveFeedForwardSettings,
                         FeedForwardSettings turnFeedForwardSettings) {
@@ -57,22 +59,23 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.cancoderInverted = cancoderInverted;
         this.driveInverted = driveInverted;
         this.offset = offset;
-        this.driveEncoder = driveEncoder;
-        this.turnEncoder = turnEncoder;
+        this.turnEncoder = turnMotor.getEncoder();
         this.drivePIDSettings = drivePIDSettings;
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
+        MotorOutput = new MotorOutputConfigs();
         turnEncoderConfig = new EncoderConfig();
-        driveEncoderConfig = new EncoderConfig();
         createDriveConfig();
         createTurnController();
         configureAbsoluteEncoder();
     }
 
     private void createDriveConfig() {
-        driveEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI);
-        driveEncoderConfig.velocityConversionFactor((DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI) / SECONDS_IN_MINUTE);
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        //@TODO check if this is correct
+        config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI;
+        config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI) / SECONDS_IN_MINUTE;
     }
 
 
@@ -85,7 +88,8 @@ public void configureDriveController() {
         driveConfigs.kV = driveFeedForwardSettings.getkV();
         driveConfigs.kA = driveFeedForwardSettings.getkA();
         driveMotor.getConfigurator().apply(driveConfigs);
-        driveEncoderConfig.inverted(driveInverted);
+        driveMotor.getConfigurator().apply(MotorOutput.withInverted(driveInverted ?
+                InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive));
     }
 
     private void createTurnController() {
@@ -166,7 +170,6 @@ public void resetRelativeEncoder() {
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);
-        namespace.putNumber("velocity", driveEncoder::getVelocity);
         namespace.putNumber("relative angle", turnEncoder::getPosition);
     }
 }

From 78708e0e9e29c3578e2d151d43fe97a7a4f8ff73 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 17:13:26 +0200
Subject: [PATCH 007/158] created SwerveModuleHolder.java

---
 .../robot/subsystems/SwerveModuleHolder.java  | 93 +++++++++++++++++++
 1 file changed, 93 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
new file mode 100644
index 0000000..abf901b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -0,0 +1,93 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.spark.SparkLowLevel;
+import com.revrobotics.spark.SparkMax;
+import com.spikes2212.control.FeedForwardSettings;
+import com.spikes2212.control.PIDSettings;
+import com.spikes2212.dashboard.RootNamespace;
+import frc.robot.RobotMap;
+
+public class SwerveModuleHolder {
+
+    private static final RootNamespace namespace = new RootNamespace("swerve module holder");
+
+    private static final boolean FRONT_LEFT_DRIVE_INVERTED = false;
+    private static final boolean FRONT_RIGHT_DRIVE_INVERTED = false;
+    private static final boolean BACK_LEFT_DRIVE_INVERTED = false;
+    private static final boolean BACK_RIGHT_DRIVE_INVERTED = false;
+
+    private static final boolean FRONT_LEFT_CANCODER_INVERTED = false;
+    private static final boolean FRONT_RIGHT_CANCODER_INVERTED = false;
+    private static final boolean BACK_LEFT_CANCODER_INVERTED = false;
+    private static final boolean BACK_RIGHT_CANCODER_INVERTED = false;
+
+    private static final String FRONT_LEFT_NAMESPACE_NAME = "front left";
+    private static final String FRONT_RIGHT_NAMESPACE_NAME = "front right";
+    private static final String BACK_LEFT_NAMESPACE_NAME = "back left";
+    private static final String BACK_RIGHT_NAMESPACE_NAME = "back right";
+
+    private static final double FRONT_LEFT_OFFSET = -1;
+    private static final double FRONT_RIGHT_OFFSET = -1;
+    private static final double BACK_LEFT_OFFSET = -1;
+    private static final double BACK_RIGHT_OFFSET = -1;
+
+    private static final PIDSettings drivePIDSetting = namespace.addPIDNamespace("drive",
+            new PIDSettings(-1, -1, -1, -1, -1));
+    private static final PIDSettings turnPIDSetting = namespace.addPIDNamespace("turn",
+            new PIDSettings(-1, -1, -1, -1, -1));
+    private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
+            "drive", new FeedForwardSettings(-1, -1, -1, -1));
+    private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
+            "turn", new FeedForwardSettings(-1, -1, -1, -1));
+
+    private static SwerveModule frontLeft;
+    private static SwerveModule frontRight;
+    private static SwerveModule backLeft;
+    private static SwerveModule backRight;
+
+    public static SwerveModule getFrontLeft() {
+        if (frontLeft == null) {
+            frontLeft = new SwerveModule(FRONT_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_LEFT_DRIVE_TALON_FX),
+                    new SparkMax(RobotMap.CAN.FRONT_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+                    new CANcoder(RobotMap.CAN.FRONT_LEFT_ABSOLUTE_ENCODER),
+                    FRONT_LEFT_CANCODER_INVERTED, FRONT_LEFT_DRIVE_INVERTED, FRONT_LEFT_OFFSET, drivePIDSetting,
+                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+        }
+        return frontLeft;
+    }
+
+    public static SwerveModule getFrontRight() {
+        if (frontRight == null) {
+            frontRight = new SwerveModule(FRONT_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_RIGHT_DRIVE_TALON_FX),
+                    new SparkMax(RobotMap.CAN.FRONT_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+                    new CANcoder(RobotMap.CAN.FRONT_RIGHT_ABSOLUTE_ENCODER),
+                    FRONT_RIGHT_CANCODER_INVERTED, FRONT_RIGHT_DRIVE_INVERTED, FRONT_RIGHT_OFFSET, drivePIDSetting,
+                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+        }
+        return frontRight;
+    }
+
+    public static SwerveModule getBackLeft() {
+        if (backLeft == null) {
+            backLeft = new SwerveModule(BACK_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_LEFT_DRIVE_TALON_FX),
+                    new SparkMax(RobotMap.CAN.BACK_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+                    new CANcoder(RobotMap.CAN.BACK_LEFT_ABSOLUTE_ENCODER),
+                    BACK_LEFT_CANCODER_INVERTED, BACK_LEFT_DRIVE_INVERTED, BACK_LEFT_OFFSET, drivePIDSetting,
+                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+        }
+        return backLeft;
+    }
+
+    public static SwerveModule getBackRight() {
+        if (backRight == null) {
+            backRight = new SwerveModule(BACK_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_RIGHT_DRIVE_TALON_FX),
+                    new SparkMax(RobotMap.CAN.BACK_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+                    new CANcoder(RobotMap.CAN.BACK_RIGHT_ABSOLUTE_ENCODER),
+                    BACK_RIGHT_CANCODER_INVERTED, BACK_RIGHT_DRIVE_INVERTED, BACK_RIGHT_OFFSET, drivePIDSetting,
+                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+        }
+        return backRight;
+    }
+}

From c570351612a40abeb2725bd842aa0976804ae8bf Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 19:14:24 +0200
Subject: [PATCH 008/158] removed blank lines. added blank lines. added limit
 speed accelerations in Drive.java to xSpeed, ySpeed and rotationSpeed.
 changed track width and track length to equal -1 in Drivetrain.java. merged
 create configuration functions with their configure controllers counterpart.
 applied configurations to their motor controllers. added feed forward
 configuration function. added feed forward to set angle. added s to
 drivePIDSetting and turnPIDSetting in SwerveModuleHolder.java.

---
 src/main/java/frc/robot/RobotMap.java         |  1 -
 src/main/java/frc/robot/commands/Drive.java   | 21 +++++++--
 .../java/frc/robot/subsystems/Drivetrain.java |  6 +--
 .../frc/robot/subsystems/SwerveModule.java    | 43 +++++++++++++------
 .../robot/subsystems/SwerveModuleHolder.java  | 20 ++++-----
 5 files changed, 60 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 1633428..3f149cc 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -18,7 +18,6 @@ public interface CAN {
         int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
         int BACK_LEFT_ABSOLUTE_ENCODER = -1;
         int BACK_RIGHT_ABSOLUTE_ENCODER = -1;
-
     }
     
     public interface DIO {
diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index c5f61fa..7f30c62 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -1,11 +1,16 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Drivetrain;
 
 import java.util.function.Supplier;
 
 public class Drive extends Command {
+
+    private final double DRIVE_ACCELERATION_LIMIT = 2;
+    private final double TURN_ACCELERATION_LIMIT = 4;
+
     private final Drivetrain drivetrain;
     private final Supplier<Double> xSpeed;
     private final Supplier<Double> ySpeed;
@@ -13,20 +18,28 @@ public class Drive extends Command {
     private final boolean fieldRelative;
     private final boolean usePID;
 
-    public Drive( Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
+    private final SlewRateLimiter xLimiter;
+    private final SlewRateLimiter yLimiter;
+    private final SlewRateLimiter rotationLimiter;
+
+    public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
+                 Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
         this.drivetrain = drivetrain;
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationsSpeed = rotationsSpeed;
         this.fieldRelative = fieldRelative;
         this.usePID = usePID;
+        this.xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        this.yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        this.rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
     }
 
     @Override
     public void execute() {
-        double xSpeed = this.xSpeed.get();
-        double ySpeed = this.ySpeed.get();
-        double rotationSpeed = this.rotationsSpeed.get();
+        double xSpeed = xLimiter.calculate(this.xSpeed.get());
+        double ySpeed = yLimiter.calculate(this.ySpeed.get());
+        double rotationSpeed = rotationLimiter.calculate(this.rotationsSpeed.get());
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 86d38cb..1e1dfe4 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -9,8 +9,9 @@
 import frc.robot.subsystems.SwerveModule;
 
 public class Drivetrain extends DashboardedSubsystem {
-    private static final double TRACK_WIDTH = 0.61;
-    private static final double TRACK_LENGTH = 0.58;
+
+    private static final double TRACK_WIDTH = -1;
+    private static final double TRACK_LENGTH = -1;
 
     public static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     public static final Translation2d FRONT_LEFT_WHEEL_POSITION =
@@ -78,6 +79,5 @@ public void resetRelativeEncoder() {
 
     @Override
     public void configureDashboard() {
-
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 1ab34cd..ffd887a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -10,11 +10,15 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
 import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -25,6 +29,7 @@ public class SwerveModule extends DashboardedSubsystem {
     public static final double MAX_SPEED = 4;
     public static final double MIN_SPEED = 0.4;
     public static final double MAX_TURN = 3;
+
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
     private static final double TURN_GEAR_RATIO = 1 / 12.8;
     private static final double WHEEL_DIAMETER = 4 * 0.0254;
@@ -32,19 +37,24 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double SECONDS_IN_MINUTE = 60;
     private static final double ABSOLUTE_POSITION = 1;
 
-
     private final TalonFX driveMotor;
     private final SparkMax turnMotor;
     private final CANcoder absoluteEncoder;
+
     private final boolean cancoderInverted;
     private final boolean driveInverted;
     private final double offset;
+
     private final RelativeEncoder turnEncoder;
+
     private final PIDSettings drivePIDSettings;
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
+    private final FeedForwardController turnFeedForwardController;
+
     private final EncoderConfig turnEncoderConfig;
+    private final SparkMaxConfig sparkConfig;
     private final MotorOutputConfigs MotorOutput;
 
     public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CANcoder absoluteEncoder,
@@ -64,22 +74,22 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
+        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings,
+                FeedForwardController.DEFAULT_PERIOD);
+        this.sparkConfig = new SparkMaxConfig();
         MotorOutput = new MotorOutputConfigs();
         turnEncoderConfig = new EncoderConfig();
-        createDriveConfig();
-        createTurnController();
+        configureDriveController();
+        configureTurnController();
         configureAbsoluteEncoder();
     }
 
-    private void createDriveConfig() {
+    public void configureDriveController() {
         TalonFXConfiguration config = new TalonFXConfiguration();
         //@TODO check if this is correct
         config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI;
         config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI) / SECONDS_IN_MINUTE;
-    }
-
-
-    public void configureDriveController() {
+        driveMotor.getConfigurator().apply(config);
         Slot0Configs driveConfigs = new Slot0Configs();
         driveConfigs.kP = drivePIDSettings.getkP();
         driveConfigs.kI = drivePIDSettings.getkI();
@@ -92,15 +102,15 @@ public void configureDriveController() {
                 InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive));
     }
 
-    private void createTurnController() {
+    public void configureTurnController() {
         turnEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * DEGREES_IN_ROTATIONS);
         turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
-    }
-
-    public void configureTurnController() {
         ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
         turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());
         turnEncoderConfig.inverted(cancoderInverted);
+        sparkConfig.apply(turnClosedLoopConfig);
+        turnMotor.configure(sparkConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+
     }
 
     public void configureAbsoluteEncoder() {
@@ -111,6 +121,10 @@ public void configureAbsoluteEncoder() {
         absoluteEncoder.getConfigurator().apply(magnetConfigs);
     }
 
+    private void turnConfigureFF() {
+        turnFeedForwardController.setGains(turnFeedForwardSettings);
+    }
+
     private void setSpeed(double speed, boolean usePID) {
         if (usePID) {
             configureDriveController();
@@ -120,7 +134,10 @@ private void setSpeed(double speed, boolean usePID) {
 
     private void setAngle(double angle) {
         configureTurnController();
-        turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity);
+        turnConfigureFF();
+        double feedForward = turnFeedForwardController.calculate(angle);
+        turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity, ClosedLoopSlot.kSlot0,
+                feedForward, SparkClosedLoopController.ArbFFUnits.kPercentOut);
     }
 
     public void stop() {
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index abf901b..ed6fc7f 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -33,9 +33,9 @@ public class SwerveModuleHolder {
     private static final double BACK_LEFT_OFFSET = -1;
     private static final double BACK_RIGHT_OFFSET = -1;
 
-    private static final PIDSettings drivePIDSetting = namespace.addPIDNamespace("drive",
+    private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
             new PIDSettings(-1, -1, -1, -1, -1));
-    private static final PIDSettings turnPIDSetting = namespace.addPIDNamespace("turn",
+    private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
             new PIDSettings(-1, -1, -1, -1, -1));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
             "drive", new FeedForwardSettings(-1, -1, -1, -1));
@@ -52,8 +52,8 @@ public static SwerveModule getFrontLeft() {
             frontLeft = new SwerveModule(FRONT_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_LEFT_DRIVE_TALON_FX),
                     new SparkMax(RobotMap.CAN.FRONT_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.FRONT_LEFT_ABSOLUTE_ENCODER),
-                    FRONT_LEFT_CANCODER_INVERTED, FRONT_LEFT_DRIVE_INVERTED, FRONT_LEFT_OFFSET, drivePIDSetting,
-                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+                    FRONT_LEFT_CANCODER_INVERTED, FRONT_LEFT_DRIVE_INVERTED, FRONT_LEFT_OFFSET, drivePIDSettings,
+                    turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
         }
         return frontLeft;
     }
@@ -63,8 +63,8 @@ public static SwerveModule getFrontRight() {
             frontRight = new SwerveModule(FRONT_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_RIGHT_DRIVE_TALON_FX),
                     new SparkMax(RobotMap.CAN.FRONT_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.FRONT_RIGHT_ABSOLUTE_ENCODER),
-                    FRONT_RIGHT_CANCODER_INVERTED, FRONT_RIGHT_DRIVE_INVERTED, FRONT_RIGHT_OFFSET, drivePIDSetting,
-                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+                    FRONT_RIGHT_CANCODER_INVERTED, FRONT_RIGHT_DRIVE_INVERTED, FRONT_RIGHT_OFFSET, drivePIDSettings,
+                    turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
         }
         return frontRight;
     }
@@ -74,8 +74,8 @@ public static SwerveModule getBackLeft() {
             backLeft = new SwerveModule(BACK_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_LEFT_DRIVE_TALON_FX),
                     new SparkMax(RobotMap.CAN.BACK_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.BACK_LEFT_ABSOLUTE_ENCODER),
-                    BACK_LEFT_CANCODER_INVERTED, BACK_LEFT_DRIVE_INVERTED, BACK_LEFT_OFFSET, drivePIDSetting,
-                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+                    BACK_LEFT_CANCODER_INVERTED, BACK_LEFT_DRIVE_INVERTED, BACK_LEFT_OFFSET, drivePIDSettings,
+                    turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
         }
         return backLeft;
     }
@@ -85,8 +85,8 @@ public static SwerveModule getBackRight() {
             backRight = new SwerveModule(BACK_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_RIGHT_DRIVE_TALON_FX),
                     new SparkMax(RobotMap.CAN.BACK_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.BACK_RIGHT_ABSOLUTE_ENCODER),
-                    BACK_RIGHT_CANCODER_INVERTED, BACK_RIGHT_DRIVE_INVERTED, BACK_RIGHT_OFFSET, drivePIDSetting,
-                    turnPIDSetting, driveFeedForwardSettings, turnFeedForwardSettings);
+                    BACK_RIGHT_CANCODER_INVERTED, BACK_RIGHT_DRIVE_INVERTED, BACK_RIGHT_OFFSET, drivePIDSettings,
+                    turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
         }
         return backRight;
     }

From 2723810b6a591621ae1260b5293e1d827ca710d4 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 20:10:48 +0200
Subject: [PATCH 009/158] renamed MAX_TURN to MAX_TURN_SPEED

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index ffd887a..d5b1352 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -28,7 +28,7 @@ public class SwerveModule extends DashboardedSubsystem {
 
     public static final double MAX_SPEED = 4;
     public static final double MIN_SPEED = 0.4;
-    public static final double MAX_TURN = 3;
+    public static final double MAX_TURN_SPEED = 3;
 
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
     private static final double TURN_GEAR_RATIO = 1 / 12.8;

From 2666684930676635dfe1fbed0329c8699f53e47a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 20:11:26 +0200
Subject: [PATCH 010/158] removed blank line

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index d5b1352..477ea5b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -110,7 +110,6 @@ public void configureTurnController() {
         turnEncoderConfig.inverted(cancoderInverted);
         sparkConfig.apply(turnClosedLoopConfig);
         turnMotor.configure(sparkConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
-
     }
 
     public void configureAbsoluteEncoder() {

From 26667dea342bf6321d00b0abdb2624ad4513448a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 20:14:56 +0200
Subject: [PATCH 011/158] unmagiced

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 477ea5b..16bbf77 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -37,6 +37,8 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double SECONDS_IN_MINUTE = 60;
     private static final double ABSOLUTE_POSITION = 1;
 
+    private static final double MAX_DISTANCE_TO_ROTATE = 90;
+
     private final TalonFX driveMotor;
     private final SparkMax turnMotor;
     private final CANcoder absoluteEncoder;
@@ -163,7 +165,7 @@ private double normalizeAngleRelativeToEncoder(double currentAngle, double desir
 
     private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
         double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
-        while (Math.abs(desiredAngle - currentAngle) > 90) {
+        while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= 180;
             }

From 9507f078029fc1fe224f14ff771df1b405306ff8 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 20:16:27 +0200
Subject: [PATCH 012/158] unmagiced 2.0

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 16bbf77..0749ff8 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -38,6 +38,7 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double ABSOLUTE_POSITION = 1;
 
     private static final double MAX_DISTANCE_TO_ROTATE = 90;
+    private static final double DEGREES_TO_FLIP = 180;
 
     private final TalonFX driveMotor;
     private final SparkMax turnMotor;
@@ -167,10 +168,10 @@ private SwerveModuleState optimize(SwerveModuleState state, double currentAngle)
         double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
         while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
-                desiredAngle -= 180;
+                desiredAngle -= DEGREES_TO_FLIP;
             }
             else {
-                desiredAngle += 180;
+                desiredAngle += DEGREES_TO_FLIP;
             }
             state.speedMetersPerSecond *= -1;
         }

From 62e04c86f6258d983d5b52a2ad329104ccfd7676 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sun, 19 Jan 2025 20:24:00 +0200
Subject: [PATCH 013/158] renamed ABSOLUTE_POSITION to
 ABSOLUTE_POSITION_DISCONTINUITY_POINT

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 0749ff8..a3dfef6 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -35,7 +35,7 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double WHEEL_DIAMETER = 4 * 0.0254;
     private static final double DEGREES_IN_ROTATIONS = 360;
     private static final double SECONDS_IN_MINUTE = 60;
-    private static final double ABSOLUTE_POSITION = 1;
+    private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
 
     private static final double MAX_DISTANCE_TO_ROTATE = 90;
     private static final double DEGREES_TO_FLIP = 180;
@@ -117,7 +117,7 @@ public void configureTurnController() {
 
     public void configureAbsoluteEncoder() {
         MagnetSensorConfigs magnetConfigs = new MagnetSensorConfigs()
-                .withAbsoluteSensorDiscontinuityPoint(ABSOLUTE_POSITION)
+                .withAbsoluteSensorDiscontinuityPoint(ABSOLUTE_POSITION_DISCONTINUITY_POINT)
                 .withSensorDirection(cancoderInverted ? SensorDirectionValue.Clockwise_Positive :
                         SensorDirectionValue.CounterClockwise_Positive).withMagnetOffset(offset);
         absoluteEncoder.getConfigurator().apply(magnetConfigs);

From ad643e92e3b34d3dd3dd2d043e8fc1e60f1071c6 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 16:47:55 +0200
Subject: [PATCH 014/158] made Translation2d fields private moved gyro ahead of
 SwerveModules removes this from kinematics

---
 .../java/frc/robot/subsystems/Drivetrain.java     | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 1e1dfe4..36c4b61 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,24 +13,25 @@ public class Drivetrain extends DashboardedSubsystem {
     private static final double TRACK_WIDTH = -1;
     private static final double TRACK_LENGTH = -1;
 
-    public static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
-    public static final Translation2d FRONT_LEFT_WHEEL_POSITION =
+    private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
+    private static final Translation2d FRONT_LEFT_WHEEL_POSITION =
             new Translation2d(TRACK_WIDTH / 2, TRACK_LENGTH / 2);
-    public static final Translation2d FRONT_RIGHT_WHEEL_POSITION =
+    private static final Translation2d FRONT_RIGHT_WHEEL_POSITION =
             new Translation2d(TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
-    public static final Translation2d BACK_LEFT_WHEEL_POSITION =
+    private static final Translation2d BACK_LEFT_WHEEL_POSITION =
             new Translation2d(-TRACK_WIDTH / 2, TRACK_LENGTH / 2);
-    public static final Translation2d BACK_RIGHT_WHEEL_POSITION =
+    private static final Translation2d BACK_RIGHT_WHEEL_POSITION =
             new Translation2d(-TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
 
     private static final String NAMESPACE_NAME = "drivetrain";
 
+    private final AHRS gyro;
+
     private final SwerveModule frontLeft;
     private final SwerveModule frontRight;
     private final SwerveModule backLeft;
     private final SwerveModule backRight;
     private final SwerveDriveKinematics kinematics;
-    private final AHRS gyro;
 
     public Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
                       SwerveModule backRight, AHRS gyro) {
@@ -39,7 +40,7 @@ public Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
-        this.kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
+        kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         this.gyro = gyro;
     }

From 9a55578fb9d2f577127826209e410a22004f887d Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 16:50:23 +0200
Subject: [PATCH 015/158] max and min speeds moved from SwerveModule.java to
 Drivetrain.java

---
 src/main/java/frc/robot/subsystems/Drivetrain.java   | 7 +++++--
 src/main/java/frc/robot/subsystems/SwerveModule.java | 8 ++------
 2 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 36c4b61..9c4560c 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -6,10 +6,13 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import frc.robot.subsystems.SwerveModule;
 
 public class Drivetrain extends DashboardedSubsystem {
 
+    public static final double MAX_SPEED = 4;
+    public static final double MIN_SPEED = 0.4;
+    public static final double MAX_TURN_SPEED = 3;
+
     private static final double TRACK_WIDTH = -1;
     private static final double TRACK_LENGTH = -1;
 
@@ -56,7 +59,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
             speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
         }
         SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, CENTER_OF_ROBOT);
-        SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveModule.MAX_SPEED);
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_SPEED);
         frontLeft.set(states[0], usePID);
         frontRight.set(states[1], usePID);
         backLeft.set(states[2], usePID);
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index a3dfef6..65f9d56 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -26,10 +26,6 @@
 
 public class SwerveModule extends DashboardedSubsystem {
 
-    public static final double MAX_SPEED = 4;
-    public static final double MIN_SPEED = 0.4;
-    public static final double MAX_TURN_SPEED = 3;
-
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
     private static final double TURN_GEAR_RATIO = 1 / 12.8;
     private static final double WHEEL_DIAMETER = 4 * 0.0254;
@@ -131,7 +127,7 @@ private void setSpeed(double speed, boolean usePID) {
         if (usePID) {
             configureDriveController();
             driveMotor.setControl(new VelocityDutyCycle(speed));
-        } else driveMotor.set(speed / MAX_SPEED);
+        } else driveMotor.set(speed / Drivetrain.MAX_SPEED);
     }
 
     private void setAngle(double angle) {
@@ -148,7 +144,7 @@ public void stop() {
     }
 
     public void set(SwerveModuleState state, boolean usePID) {
-        if (state.speedMetersPerSecond < MIN_SPEED){
+        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED){
             stop();
             return;
         }

From 0de041aef3b81551b313cd3ab702388b740ad9bf Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 16:55:25 +0200
Subject: [PATCH 016/158] separated WHEEL_DIAMETER into WHEEL_DIAMETER_INCHES
 and INCHES_TO_METERS

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 65f9d56..37701cc 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -28,7 +28,8 @@ public class SwerveModule extends DashboardedSubsystem {
 
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
     private static final double TURN_GEAR_RATIO = 1 / 12.8;
-    private static final double WHEEL_DIAMETER = 4 * 0.0254;
+    private static final double WHEEL_DIAMETER_INCHES = 4;
+    private static final double INCHES_TO_METERS = 0.0254;
     private static final double DEGREES_IN_ROTATIONS = 360;
     private static final double SECONDS_IN_MINUTE = 60;
     private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
@@ -86,8 +87,10 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
     public void configureDriveController() {
         TalonFXConfiguration config = new TalonFXConfiguration();
         //@TODO check if this is correct
-        config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI;
-        config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI) / SECONDS_IN_MINUTE;
+        config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
+                * INCHES_TO_METERS * Math.PI;
+        config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
+                * INCHES_TO_METERS * Math.PI) / SECONDS_IN_MINUTE;
         driveMotor.getConfigurator().apply(config);
         Slot0Configs driveConfigs = new Slot0Configs();
         driveConfigs.kP = drivePIDSettings.getkP();

From bd0ba6805ad610a87ccf1ff7553d5f14e0b459db Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 16:57:05 +0200
Subject: [PATCH 017/158] put ABSOLUTE_POSITION_DISCONTINUITY_POINT below
 DEGREES_TO_FLIP

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 37701cc..5bdf40a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -32,10 +32,10 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double INCHES_TO_METERS = 0.0254;
     private static final double DEGREES_IN_ROTATIONS = 360;
     private static final double SECONDS_IN_MINUTE = 60;
-    private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
 
     private static final double MAX_DISTANCE_TO_ROTATE = 90;
     private static final double DEGREES_TO_FLIP = 180;
+    private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
 
     private final TalonFX driveMotor;
     private final SparkMax turnMotor;

From d69be6d1bb63abfaf2f45b3c9615f281be4cd3d5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 16:58:16 +0200
Subject: [PATCH 018/158] put turnEncoder below turnFeedForwardController

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 5bdf40a..867cb0b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -45,13 +45,13 @@ public class SwerveModule extends DashboardedSubsystem {
     private final boolean driveInverted;
     private final double offset;
 
-    private final RelativeEncoder turnEncoder;
 
     private final PIDSettings drivePIDSettings;
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
     private final FeedForwardController turnFeedForwardController;
+    private final RelativeEncoder turnEncoder;
 
     private final EncoderConfig turnEncoderConfig;
     private final SparkMaxConfig sparkConfig;

From 0ae2b1edbcbefd106766ccae2aeacb5c864c0161 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 16:59:09 +0200
Subject: [PATCH 019/158] ditto

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 867cb0b..94d391b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -69,13 +69,13 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.cancoderInverted = cancoderInverted;
         this.driveInverted = driveInverted;
         this.offset = offset;
-        this.turnEncoder = turnMotor.getEncoder();
         this.drivePIDSettings = drivePIDSettings;
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
         this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings,
                 FeedForwardController.DEFAULT_PERIOD);
+        this.turnEncoder = turnMotor.getEncoder();
         this.sparkConfig = new SparkMaxConfig();
         MotorOutput = new MotorOutputConfigs();
         turnEncoderConfig = new EncoderConfig();

From 0c2e260a83b65de1d69b46af41e175166dd27b0a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Mon, 20 Jan 2025 17:42:09 +0200
Subject: [PATCH 020/158] turned Drivetrain.java into a singleton class

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 14 +++++++++++++-
 .../java/frc/robot/subsystems/SwerveModule.java    |  4 ++++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9c4560c..2238e4c 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SerialPort;
 
 public class Drivetrain extends DashboardedSubsystem {
 
@@ -36,7 +37,18 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveModule backRight;
     private final SwerveDriveKinematics kinematics;
 
-    public Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
+    private static Drivetrain instance;
+
+    public static Drivetrain getInstance() {
+        if (instance == null) {
+            instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
+                    SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
+                    new AHRS(AHRS.NavXComType.kI2C));
+        }
+        return instance;
+    }
+
+    private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
                       SwerveModule backRight, AHRS gyro) {
         super(NAMESPACE_NAME);
         this.frontLeft = frontLeft;
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 94d391b..7186562 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -21,8 +21,12 @@
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
+import com.studica.frc.AHRS;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SerialPort;
+
+import java.io.Serial;
 
 public class SwerveModule extends DashboardedSubsystem {
 

From b0b90fffce5493c2ca1ced7c5cbf058d927af5fd Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 11:54:30 +0200
Subject: [PATCH 021/158] committing just so I can make changes in Swerve

---
 .../deploy/pathplanner/autos/New Auto.auto    | 25 ++++++
 src/main/deploy/pathplanner/navgrid.json      |  1 +
 .../pathplanner/paths/Example Path.path       | 54 +++++++++++++
 .../deploy/pathplanner/paths/New Path.path    | 54 +++++++++++++
 src/main/deploy/pathplanner/settings.json     | 32 ++++++++
 .../java/frc/robot/subsystems/Drivetrain.java | 78 +++++++++++++++++--
 .../frc/robot/subsystems/SwerveModule.java    | 17 +++-
 7 files changed, 252 insertions(+), 9 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 create mode 100644 src/main/deploy/pathplanner/navgrid.json
 create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path
 create mode 100644 src/main/deploy/pathplanner/paths/New Path.path
 create mode 100644 src/main/deploy/pathplanner/settings.json

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
new file mode 100644
index 0000000..64497c1
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 0000000..6730296
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.417329545452214,
+        "y": 5.023626893946309
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6678597876779695,
+        "y": 5.9755388574449535
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.648153409088578,
+        "y": 3.2169507575826723
+      },
+      "prevControl": {
+        "x": 6.48709732984848,
+        "y": 1.358194205188759
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "blop"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": 107.02052561148975
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -135.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 0000000..5cc35f8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.648153409088578,
+        "y": 3.2169507575826723
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.921212121212121,
+        "y": 5.641098484848485
+      },
+      "isLocked": false,
+      "linkedName": "blop"
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 5.199763257575757,
+        "y": 7.356297348484849
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 107.02052561148976
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..2708174
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 5.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 40.0,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.051,
+  "driveGearing": 6.12,
+  "maxDriveSpeed": 5.0,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.15,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 2238e4c..90f70dc 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,12 +1,16 @@
 package frc.robot.subsystems;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.spikes2212.command.DashboardedSubsystem;
 import com.studica.frc.AHRS;
+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.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.math.kinematics.*;
+import edu.wpi.first.wpilibj.DriverStation;
 
 public class Drivetrain extends DashboardedSubsystem {
 
@@ -29,14 +33,16 @@ public class Drivetrain extends DashboardedSubsystem {
 
     private static final String NAMESPACE_NAME = "drivetrain";
 
-    private final AHRS gyro;
 
     private final SwerveModule frontLeft;
     private final SwerveModule frontRight;
     private final SwerveModule backLeft;
     private final SwerveModule backRight;
+    private final AHRS gyro;
     private final SwerveDriveKinematics kinematics;
 
+    private SwerveModulePosition[] stupidPointlessArray;
+    private final SwerveDriveOdometry odometry;
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {
@@ -55,9 +61,54 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
+        stupidPointlessArray = new SwerveModulePosition[]{frontLeft.getPosition(),
+                frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         this.gyro = gyro;
+        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), stupidPointlessArray,
+                new Pose2d());
+        RobotConfig config;
+        try{
+            config = RobotConfig.fromGUISettings();
+        } catch (Exception e) {
+            // Handle exception as needed
+            e.printStackTrace();
+            config = null;
+        }
+
+        // Configure AutoBuilder last
+        AutoBuilder.configure(
+                this::getPose2d, // Robot pose supplier
+                this::resetPose2d, // Method to reset odometry (will be called if your auto has a starting pose)
+                this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+                ),
+                config, // The robot configuration
+                () -> {
+                    // Boolean supplier that controls when the path will be mirrored for the red alliance
+                    // This will flip the path being followed to the red side of the field.
+                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                    var alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+                },
+                this // Reference to this subsystem to set requirements
+        );
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+        stupidPointlessArray = new SwerveModulePosition[]{frontLeft.getPosition(),
+                frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
+        odometry.update(gyro.getRotation2d(), stupidPointlessArray);
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
@@ -92,6 +143,23 @@ public void resetRelativeEncoder() {
         backRight.resetRelativeEncoder();
     }
 
+    public Pose2d getPose2d() {
+        return odometry.getPoseMeters();
+    }
+
+    public void resetPose2d(Pose2d desiredPose) {
+        odometry.resetPosition(gyro.getRotation2d(), stupidPointlessArray, desiredPose);
+    }
+
+    public ChassisSpeeds getRobotRelativeSpeeds() {
+        return kinematics.toChassisSpeeds(frontLeft.getState(), frontRight.getState(),
+                backLeft.getState(), backRight.getState());
+    }
+
+    public void driveRobotRelative(ChassisSpeeds speeds) {
+        drive(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond,
+                false, true);
+    }
 
     @Override
     public void configureDashboard() {
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 7186562..53b39b2 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -21,12 +21,9 @@
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
-import com.studica.frc.AHRS;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.SerialPort;
-
-import java.io.Serial;
 
 public class SwerveModule extends DashboardedSubsystem {
 
@@ -189,6 +186,18 @@ public void resetRelativeEncoder() {
         turnEncoder.setPosition(getAbsoluteAngle());
     }
 
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(driveMotor.getPosition().getValueAsDouble(),
+                Rotation2d.fromDegrees(getAbsoluteAngle()));
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(driveMotor.getVelocity().getValueAsDouble(),
+                Rotation2d.fromDegrees(getAbsoluteAngle()));
+    }
+
+
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);

From 2c075e3f656c6442b98cd75c7b49e652d3ea6ce5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:03:47 +0200
Subject: [PATCH 022/158] moved the gyro field right below the modules

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 2238e4c..11161eb 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -29,12 +29,12 @@ public class Drivetrain extends DashboardedSubsystem {
 
     private static final String NAMESPACE_NAME = "drivetrain";
 
-    private final AHRS gyro;
 
     private final SwerveModule frontLeft;
     private final SwerveModule frontRight;
     private final SwerveModule backLeft;
     private final SwerveModule backRight;
+    private final AHRS gyro;
     private final SwerveDriveKinematics kinematics;
 
     private static Drivetrain instance;

From f28c183bc7a67406246b1d8d02516a4a7aa05390 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:06:02 +0200
Subject: [PATCH 023/158] swapped the gyro initializer with the kinematics
 initializer

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 11161eb..006a389 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -55,9 +55,9 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
+        this.gyro = gyro;
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
-        this.gyro = gyro;
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,

From 6c9c0278de94e4f5024b0700d1be664efe86eb35 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:06:34 +0200
Subject: [PATCH 024/158] removed blank line

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 7186562..4f54139 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -49,7 +49,6 @@ public class SwerveModule extends DashboardedSubsystem {
     private final boolean driveInverted;
     private final double offset;
 
-
     private final PIDSettings drivePIDSettings;
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;

From e17c3a24948f128bddad0d48ab1331e46d92b0d3 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:09:05 +0200
Subject: [PATCH 025/158] separated the turnEncoder from the control loop
 related fields

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 4f54139..291c106 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -54,6 +54,7 @@ public class SwerveModule extends DashboardedSubsystem {
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
     private final FeedForwardController turnFeedForwardController;
+
     private final RelativeEncoder turnEncoder;
 
     private final EncoderConfig turnEncoderConfig;

From a0a05a855883a204539f0d752dfc8370c8ffd698 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:43:31 +0200
Subject: [PATCH 026/158] added a todo to check if new AHRS is correct as well
 as adding gyro angle to the dashboard

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 006a389..d9800eb 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -43,6 +43,7 @@ public static Drivetrain getInstance() {
         if (instance == null) {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
+                    //TODO check in shuffleboard if this is correct
                     new AHRS(AHRS.NavXComType.kI2C));
         }
         return instance;
@@ -95,5 +96,6 @@ public void resetRelativeEncoder() {
 
     @Override
     public void configureDashboard() {
+        namespace.putNumber("gyro", gyro::getAngle);
     }
 }

From 101369cd37a371e2d98227621ad60a9889249271 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 24 Jan 2025 00:21:23 +0200
Subject: [PATCH 027/158] committing in order to change branch

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 291c106..2b8a67f 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -77,8 +77,7 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
-        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings,
-                FeedForwardController.DEFAULT_PERIOD);
+        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings);
         this.turnEncoder = turnMotor.getEncoder();
         this.sparkConfig = new SparkMaxConfig();
         MotorOutput = new MotorOutputConfigs();
@@ -140,7 +139,7 @@ private void setSpeed(double speed, boolean usePID) {
     private void setAngle(double angle) {
         configureTurnController();
         turnConfigureFF();
-        double feedForward = turnFeedForwardController.calculate(angle);
+        double feedForward = turnFeedForwardController.calculate( 0,angle);
         turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity, ClosedLoopSlot.kSlot0,
                 feedForward, SparkClosedLoopController.ArbFFUnits.kPercentOut);
     }

From 3b3bb8e7c734ef233d356991ba5f9c55d14de54e Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:48:51 +0200
Subject: [PATCH 028/158] added path planner

---
 .../pathplanner/paths/Example Path.path       |   4 +-
 .../deploy/pathplanner/paths/New Path.path    | 107 ++++++++++++++++--
 .../frc/robot/commands/autonomous/PPAuto.java |  10 ++
 .../java/frc/robot/subsystems/Drivetrain.java |   4 +-
 4 files changed, 111 insertions(+), 14 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/autonomous/PPAuto.java

diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
index 6730296..c489599 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 4.0,
-    "rotation": 107.02052561148975
+    "rotation": 107.02052561148976
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 3.0,
-    "rotation": -135.0
+    "rotation": 107.02052561148976
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 5cc35f8..c6f8f92 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -8,30 +8,117 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.921212121212121,
-        "y": 5.641098484848485
+        "x": 5.732007133527982,
+        "y": 5.5498307973727705
       },
       "isLocked": false,
       "linkedName": "blop"
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 4.184545836832152,
+        "y": 6.349747687125728
       },
       "prevControl": {
-        "x": 5.199763257575757,
-        "y": 7.356297348484849
+        "x": 6.258429694021986,
+        "y": 7.073495378617061
+      },
+      "nextControl": {
+        "x": 2.380660337304553,
+        "y": 5.720224504585095
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.6494533221139411,
+        "y": 2.659671993273115
+      },
+      "prevControl": {
+        "x": 0.7369234423513376,
+        "y": 1.3606143884005357
+      },
+      "nextControl": {
+        "x": 0.5171324068913985,
+        "y": 4.62482867462925
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2177249789684408,
+        "y": 6.585912531540567
+      },
+      "prevControl": {
+        "x": 2.066972297149061,
+        "y": 6.499392193224025
+      },
+      "nextControl": {
+        "x": 0.8397997012877723,
+        "y": 6.624415123404331
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.284146341457928,
+        "y": 4.512089991591029
+      },
+      "prevControl": {
+        "x": 0.7030640857270156,
+        "y": 5.380354936863563
+      },
+      "nextControl": {
+        "x": 1.8652285971888403,
+        "y": 3.6438250463184954
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.583095037846931,
+        "y": 3.1615222876366693
+      },
+      "prevControl": {
+        "x": 4.224161817517886,
+        "y": 3.4026736669775826
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1,
+      "rotationDegrees": -55.59972476898146
+    }
+  ],
   "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 5.5
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.5697674418604655,
+      "maxWaypointRelativePos": 1.365116279069768,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "",
+      "waypointRelativePos": 1.1,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 5.0,
@@ -42,7 +129,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 155.02382809380947
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/java/frc/robot/commands/autonomous/PPAuto.java b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
new file mode 100644
index 0000000..66be6f5
--- /dev/null
+++ b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
@@ -0,0 +1,10 @@
+package frc.robot.commands.autonomous;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+public class PPAuto extends PathPlannerAuto{
+
+    public PPAuto(String autoName) {
+        super(autoName);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 90f70dc..adad7e2 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -84,8 +84,8 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
                 this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
                 (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
                 new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
-                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+                        new PIDConstants(0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(0.01, 0.0, 0.0) // Rotation PID constants
                 ),
                 config, // The robot configuration
                 () -> {

From 242e3931387285b93141bdc23fec175bcda498e8 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:50:06 +0200
Subject: [PATCH 029/158] deleted extra line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index d9800eb..aee9ef5 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -29,7 +29,6 @@ public class Drivetrain extends DashboardedSubsystem {
 
     private static final String NAMESPACE_NAME = "drivetrain";
 
-
     private final SwerveModule frontLeft;
     private final SwerveModule frontRight;
     private final SwerveModule backLeft;

From 2094d3bdb9f2d37843e89e2f9fd220a1b20b09c9 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:50:58 +0200
Subject: [PATCH 030/158] changed navx port

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index aee9ef5..ec268e5 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -43,7 +43,7 @@ public static Drivetrain getInstance() {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
                     //TODO check in shuffleboard if this is correct
-                    new AHRS(AHRS.NavXComType.kI2C));
+                    new AHRS(AHRS.NavXComType.kMXP_SPI));
         }
         return instance;
     }

From 162b57cf8aa7f948b7ecba9350dbc45e3ee4b148 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:51:57 +0200
Subject: [PATCH 031/158] changed gear ratio from drive to turn

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 2b8a67f..809f886 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -108,7 +108,7 @@ public void configureDriveController() {
     }
 
     public void configureTurnController() {
-        turnEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnEncoderConfig.positionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
         turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
         ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
         turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());

From 09ab4761333c5777deade1151b6e219f95bf9ce8 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:59:17 +0200
Subject: [PATCH 032/158] updated SpikesLib2.json

---
 build.gradle               |  1 -
 vendordeps/SpikesLib2.json | 21 +++++++++++++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)
 create mode 100644 vendordeps/SpikesLib2.json

diff --git a/build.gradle b/build.gradle
index 1345de4..2e6cdec 100644
--- a/build.gradle
+++ b/build.gradle
@@ -85,7 +85,6 @@ dependencies {
     implementation group: 'com.ctre.phoenix6', name: 'wpiapi-java', version: '24.0.0-beta-8'
     implementation group: 'com.ctre.phoenix', name: 'api-java', version: '5.32.0-beta-5'
     implementation group: 'com.ctre.phoenix', name: 'wpiapi-java', version: '5.31.0'
-    implementation 'com.github.Spikes-2212-Programming-Guild:SpikesLib2:update-wpilib-25-SNAPSHOT'
 }
 
 test {
diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
new file mode 100644
index 0000000..f04853c
--- /dev/null
+++ b/vendordeps/SpikesLib2.json
@@ -0,0 +1,21 @@
+{
+  "fileName": "SpikesLib2.json",
+  "name": "SpikesLib2",
+  "version": "v5.1.0",
+  "frcYear": "2025",
+  "uuid": "018a8f54-8662-11ec-a8a3-0242ac120002",
+  "jsonUrl": "https://raw.githubusercontent.com/Spikes-2212-Programming-Guild/SpikesLib2/master/SpikesLib2.json",
+  "mavenUrls": [
+    "https://jitpack.io"
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "com.github.Spikes-2212-Programming-Guild",
+      "artifactId": "SpikesLib2",
+      "version": "v5.1.0"
+    }
+  ]
+}
\ No newline at end of file

From fc7c0a8ebc36de27bd53b1bcfd78c99606038f3c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:00:18 +0200
Subject: [PATCH 033/158] added wait correct parameters

---
 .../java/frc/robot/subsystems/SwerveModuleHolder.java    | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index ed6fc7f..2731a9c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -4,6 +4,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.spark.SparkLowLevel;
 import com.revrobotics.spark.SparkMax;
+import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.dashboard.RootNamespace;
@@ -34,13 +35,13 @@ public class SwerveModuleHolder {
     private static final double BACK_RIGHT_OFFSET = -1;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(-1, -1, -1, -1, -1));
+            new PIDSettings(-1, -1, -1, -1, -1, -1));
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(-1, -1, -1, -1, -1));
+            new PIDSettings(-1, -1, -1, -1, -1, -1));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "drive", new FeedForwardSettings(-1, -1, -1, -1));
+            "drive", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "turn", new FeedForwardSettings(-1, -1, -1, -1));
+            "turn", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_POSITION));
 
     private static SwerveModule frontLeft;
     private static SwerveModule frontRight;

From 01209260fc4eb7bf31ba04ab6b6dd12ab252a195 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:34:13 +0200
Subject: [PATCH 034/158] replaced motor controllers with wrappers

---
 .../frc/robot/subsystems/SwerveModule.java    | 79 ++++++-------------
 .../robot/subsystems/SwerveModuleHolder.java  | 18 +++--
 2 files changed, 33 insertions(+), 64 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 809f886..91f532b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -2,31 +2,19 @@
 
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.spikes2212.command.DashboardedSubsystem;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
-import com.studica.frc.AHRS;
+import com.spikes2212.util.UnifiedControlMode;
+import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.SerialPort;
-
-import java.io.Serial;
 
 public class SwerveModule extends DashboardedSubsystem {
 
@@ -41,8 +29,8 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double DEGREES_TO_FLIP = 180;
     private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
 
-    private final TalonFX driveMotor;
-    private final SparkMax turnMotor;
+    private final TalonFXWrapper driveMotor;
+    private final SparkWrapper turnMotor;
     private final CANcoder absoluteEncoder;
 
     private final boolean cancoderInverted;
@@ -55,13 +43,11 @@ public class SwerveModule extends DashboardedSubsystem {
     private final FeedForwardSettings turnFeedForwardSettings;
     private final FeedForwardController turnFeedForwardController;
 
-    private final RelativeEncoder turnEncoder;
-
     private final EncoderConfig turnEncoderConfig;
     private final SparkMaxConfig sparkConfig;
     private final MotorOutputConfigs MotorOutput;
 
-    public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CANcoder absoluteEncoder,
+    public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper turnMotor, CANcoder absoluteEncoder,
                         boolean cancoderInverted, boolean driveInverted, double offset,
                         PIDSettings drivePIDSettings, PIDSettings turnPIDSettings,
                         FeedForwardSettings driveFeedForwardSettings,
@@ -78,7 +64,6 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
         this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings);
-        this.turnEncoder = turnMotor.getEncoder();
         this.sparkConfig = new SparkMaxConfig();
         MotorOutput = new MotorOutputConfigs();
         turnEncoderConfig = new EncoderConfig();
@@ -88,33 +73,20 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
     }
 
     public void configureDriveController() {
-        TalonFXConfiguration config = new TalonFXConfiguration();
         //@TODO check if this is correct
-        config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
-                * INCHES_TO_METERS * Math.PI;
-        config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
-                * INCHES_TO_METERS * Math.PI) / SECONDS_IN_MINUTE;
-        driveMotor.getConfigurator().apply(config);
-        Slot0Configs driveConfigs = new Slot0Configs();
-        driveConfigs.kP = drivePIDSettings.getkP();
-        driveConfigs.kI = drivePIDSettings.getkI();
-        driveConfigs.kD = drivePIDSettings.getkD();
-        driveConfigs.kS = driveFeedForwardSettings.getkS();
-        driveConfigs.kV = driveFeedForwardSettings.getkV();
-        driveConfigs.kA = driveFeedForwardSettings.getkA();
-        driveMotor.getConfigurator().apply(driveConfigs);
-        driveMotor.getConfigurator().apply(MotorOutput.withInverted(driveInverted ?
-                InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive));
+        driveMotor.setEncoderConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
+                * INCHES_TO_METERS * Math.PI);
+        driveMotor.configurePID(drivePIDSettings);
+        driveMotor.configureFF(driveFeedForwardSettings);
+        driveMotor.setInverted(driveInverted);
     }
 
     public void configureTurnController() {
-        turnEncoderConfig.positionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
-        turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
-        ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
-        turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());
-        turnEncoderConfig.inverted(cancoderInverted);
-        sparkConfig.apply(turnClosedLoopConfig);
-        turnMotor.configure(sparkConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+        turnMotor.setPositionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnMotor.setVelocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
+        turnMotor.configurePID(turnPIDSettings);
+        turnMotor.setInverted(cancoderInverted);
+        turnMotor.configureFF(turnFeedForwardSettings);
     }
 
     public void configureAbsoluteEncoder() {
@@ -125,23 +97,18 @@ public void configureAbsoluteEncoder() {
         absoluteEncoder.getConfigurator().apply(magnetConfigs);
     }
 
-    private void turnConfigureFF() {
-        turnFeedForwardController.setGains(turnFeedForwardSettings);
-    }
-
     private void setSpeed(double speed, boolean usePID) {
         if (usePID) {
-            configureDriveController();
-            driveMotor.setControl(new VelocityDutyCycle(speed));
+            driveMotor.pidSet(UnifiedControlMode.VELOCITY, speed, drivePIDSettings, driveFeedForwardSettings,
+                    true);
         } else driveMotor.set(speed / Drivetrain.MAX_SPEED);
     }
 
     private void setAngle(double angle) {
         configureTurnController();
-        turnConfigureFF();
         double feedForward = turnFeedForwardController.calculate( 0,angle);
-        turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity, ClosedLoopSlot.kSlot0,
-                feedForward, SparkClosedLoopController.ArbFFUnits.kPercentOut);
+        turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings,
+                true);
     }
 
     public void stop() {
@@ -154,7 +121,7 @@ public void set(SwerveModuleState state, boolean usePID) {
             stop();
             return;
         }
-        state = optimize(state, turnEncoder.getPosition());
+        state = optimize(state, turnMotor.getPosition());
         setAngle(state.angle.getDegrees());
         setSpeed(state.speedMetersPerSecond, usePID);
     }
@@ -185,12 +152,12 @@ public double getAbsoluteAngle() {
     }
 
     public void resetRelativeEncoder() {
-        turnEncoder.setPosition(getAbsoluteAngle());
+        turnMotor.setPosition(getAbsoluteAngle());
     }
 
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);
-        namespace.putNumber("relative angle", turnEncoder::getPosition);
+        namespace.putNumber("relative angle", turnMotor::getPosition);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 2731a9c..ae328f1 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -8,6 +8,8 @@
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.dashboard.RootNamespace;
+import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
 import frc.robot.RobotMap;
 
 public class SwerveModuleHolder {
@@ -50,8 +52,8 @@ public class SwerveModuleHolder {
 
     public static SwerveModule getFrontLeft() {
         if (frontLeft == null) {
-            frontLeft = new SwerveModule(FRONT_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_LEFT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.FRONT_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            frontLeft = new SwerveModule(FRONT_LEFT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.FRONT_LEFT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.FRONT_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.FRONT_LEFT_ABSOLUTE_ENCODER),
                     FRONT_LEFT_CANCODER_INVERTED, FRONT_LEFT_DRIVE_INVERTED, FRONT_LEFT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
@@ -61,8 +63,8 @@ public static SwerveModule getFrontLeft() {
 
     public static SwerveModule getFrontRight() {
         if (frontRight == null) {
-            frontRight = new SwerveModule(FRONT_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_RIGHT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.FRONT_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            frontRight = new SwerveModule(FRONT_RIGHT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.FRONT_RIGHT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.FRONT_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.FRONT_RIGHT_ABSOLUTE_ENCODER),
                     FRONT_RIGHT_CANCODER_INVERTED, FRONT_RIGHT_DRIVE_INVERTED, FRONT_RIGHT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
@@ -72,8 +74,8 @@ public static SwerveModule getFrontRight() {
 
     public static SwerveModule getBackLeft() {
         if (backLeft == null) {
-            backLeft = new SwerveModule(BACK_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_LEFT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.BACK_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            backLeft = new SwerveModule(BACK_LEFT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.BACK_LEFT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.BACK_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.BACK_LEFT_ABSOLUTE_ENCODER),
                     BACK_LEFT_CANCODER_INVERTED, BACK_LEFT_DRIVE_INVERTED, BACK_LEFT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
@@ -83,8 +85,8 @@ public static SwerveModule getBackLeft() {
 
     public static SwerveModule getBackRight() {
         if (backRight == null) {
-            backRight = new SwerveModule(BACK_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_RIGHT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.BACK_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            backRight = new SwerveModule(BACK_RIGHT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.BACK_RIGHT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.BACK_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.BACK_RIGHT_ABSOLUTE_ENCODER),
                     BACK_RIGHT_CANCODER_INVERTED, BACK_RIGHT_DRIVE_INVERTED, BACK_RIGHT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);

From 61677e11b20b227c0a6553de6333b02d91af35b5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:38:00 +0200
Subject: [PATCH 035/158] made set angle efficient

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 91f532b..8aa059c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -105,8 +105,6 @@ private void setSpeed(double speed, boolean usePID) {
     }
 
     private void setAngle(double angle) {
-        configureTurnController();
-        double feedForward = turnFeedForwardController.calculate( 0,angle);
         turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings,
                 true);
     }

From 759cd8bae1881bfaeca72d4a1fd999e95fde5666 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:40:04 +0200
Subject: [PATCH 036/158] set default value for pid and ff to 0

---
 .../java/frc/robot/subsystems/SwerveModuleHolder.java     | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index ae328f1..474e72a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -37,13 +37,13 @@ public class SwerveModuleHolder {
     private static final double BACK_RIGHT_OFFSET = -1;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(-1, -1, -1, -1, -1, -1));
+            new PIDSettings(0, 0, 0, 0, 0,0));
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(-1, -1, -1, -1, -1, -1));
+            new PIDSettings(0, 0, 0, 0, 0,0));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "drive", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_VELOCITY));
+            "drive", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "turn", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_POSITION));
+            "turn", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_POSITION));
 
     private static SwerveModule frontLeft;
     private static SwerveModule frontRight;

From 0aea626528097169bf1215de92a556bf66c69572 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:41:33 +0200
Subject: [PATCH 037/158] reduced min speed

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index ec268e5..67e731a 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -11,7 +11,7 @@
 public class Drivetrain extends DashboardedSubsystem {
 
     public static final double MAX_SPEED = 4;
-    public static final double MIN_SPEED = 0.4;
+    public static final double MIN_SPEED = 0.05;
     public static final double MAX_TURN_SPEED = 3;
 
     private static final double TRACK_WIDTH = -1;

From 08632347c2ea76a873eeb25779a70167b2e6338f Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:52:04 +0200
Subject: [PATCH 038/158] deleted todo

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 67e731a..ebc537d 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -42,7 +42,6 @@ public static Drivetrain getInstance() {
         if (instance == null) {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
-                    //TODO check in shuffleboard if this is correct
                     new AHRS(AHRS.NavXComType.kMXP_SPI));
         }
         return instance;

From 9599b6c2038beb4239c8299d01d512cc7d3e546b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:52:31 +0200
Subject: [PATCH 039/158] deleted extra line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index ebc537d..9956270 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -91,7 +91,6 @@ public void resetRelativeEncoder() {
         backRight.resetRelativeEncoder();
     }
 
-
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro", gyro::getAngle);

From ac646964c64545e493193e89547225116a972901 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:53:39 +0200
Subject: [PATCH 040/158] specified what about the gyro

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9956270..3b1294f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -93,6 +93,6 @@ public void resetRelativeEncoder() {
 
     @Override
     public void configureDashboard() {
-        namespace.putNumber("gyro", gyro::getAngle);
+        namespace.putNumber("gyro yaw", gyro::getAngle);
     }
 }

From 59af30e809b77511dd03ca725dee5cf8f873a9f0 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:55:23 +0200
Subject: [PATCH 041/158] removed unnecessary fields

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 8aa059c..5edb131 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -41,11 +41,6 @@ public class SwerveModule extends DashboardedSubsystem {
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
-    private final FeedForwardController turnFeedForwardController;
-
-    private final EncoderConfig turnEncoderConfig;
-    private final SparkMaxConfig sparkConfig;
-    private final MotorOutputConfigs MotorOutput;
 
     public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper turnMotor, CANcoder absoluteEncoder,
                         boolean cancoderInverted, boolean driveInverted, double offset,
@@ -63,10 +58,6 @@ public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper tu
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
-        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings);
-        this.sparkConfig = new SparkMaxConfig();
-        MotorOutput = new MotorOutputConfigs();
-        turnEncoderConfig = new EncoderConfig();
         configureDriveController();
         configureTurnController();
         configureAbsoluteEncoder();

From 91b1fc8cc43e1f3928af207e2abf9e5637e0039b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 19:13:03 +0200
Subject: [PATCH 042/158] removed unnecessary pid configurations

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 5edb131..8195845 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -67,17 +67,13 @@ public void configureDriveController() {
         //@TODO check if this is correct
         driveMotor.setEncoderConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
                 * INCHES_TO_METERS * Math.PI);
-        driveMotor.configurePID(drivePIDSettings);
-        driveMotor.configureFF(driveFeedForwardSettings);
         driveMotor.setInverted(driveInverted);
     }
 
     public void configureTurnController() {
         turnMotor.setPositionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
         turnMotor.setVelocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
-        turnMotor.configurePID(turnPIDSettings);
         turnMotor.setInverted(cancoderInverted);
-        turnMotor.configureFF(turnFeedForwardSettings);
     }
 
     public void configureAbsoluteEncoder() {

From 3476693b3a6cf1ff46c1c3d3d22f996914cfebc5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 19:14:34 +0200
Subject: [PATCH 043/158] added blank line at the end of class

---
 vendordeps/SpikesLib2.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
index f04853c..bc10370 100644
--- a/vendordeps/SpikesLib2.json
+++ b/vendordeps/SpikesLib2.json
@@ -18,4 +18,4 @@
       "version": "v5.1.0"
     }
   ]
-}
\ No newline at end of file
+}

From 95d8ab0e424ac6eff17291ce270ce54f16a16ca5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 19:58:34 +0200
Subject: [PATCH 044/158] renamed reset relative encoders func

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 3b1294f..38cf0d0 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -84,7 +84,7 @@ public void stop() {
         backRight.stop();
     }
 
-    public void resetRelativeEncoder() {
+    public void resetRelativeEncoders() {
         frontLeft.resetRelativeEncoder();
         frontRight.resetRelativeEncoder();
         backLeft.resetRelativeEncoder();

From f0edeea23bf9410c521b6fd2fa8da98d719d4023 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 20:00:06 +0200
Subject: [PATCH 045/158] fixed unnecessary this call

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 7f30c62..f359e0c 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -32,7 +32,7 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
         this.usePID = usePID;
         this.xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         this.yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
-        this.rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
+        rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
     }
 
     @Override

From c336ded49639b6e28394f38f74050dc6ac0dc266 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 20:48:53 +0200
Subject: [PATCH 046/158] added track variables in cm

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 38cf0d0..3cc2747 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -14,8 +14,8 @@ public class Drivetrain extends DashboardedSubsystem {
     public static final double MIN_SPEED = 0.05;
     public static final double MAX_TURN_SPEED = 3;
 
-    private static final double TRACK_WIDTH = -1;
-    private static final double TRACK_LENGTH = -1;
+    private static final double TRACK_WIDTH = 60;
+    private static final double TRACK_LENGTH = 60;
 
     private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     private static final Translation2d FRONT_LEFT_WHEEL_POSITION =

From d5fcbbef6ad566ec05cf1ba3368a2b62a444e47b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 21:32:47 +0200
Subject: [PATCH 047/158] removed unnecessary this call

---
 src/main/java/frc/robot/commands/Drive.java | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index f359e0c..c853b57 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -25,13 +25,14 @@ public class Drive extends Command {
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
         this.drivetrain = drivetrain;
+        addRequirements(drivetrain);
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationsSpeed = rotationsSpeed;
         this.fieldRelative = fieldRelative;
         this.usePID = usePID;
-        this.xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
-        this.yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
     }
 

From 3fe06a080bc8398c8b2140de7c9f8dab9a7901c1 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:31:35 +0200
Subject: [PATCH 048/158] renamed to rotationSpeed

---
 src/main/java/frc/robot/commands/Drive.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index c853b57..9a03382 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -14,7 +14,7 @@ public class Drive extends Command {
     private final Drivetrain drivetrain;
     private final Supplier<Double> xSpeed;
     private final Supplier<Double> ySpeed;
-    private final Supplier<Double> rotationsSpeed;
+    private final Supplier<Double> rotationSpeed;
     private final boolean fieldRelative;
     private final boolean usePID;
 
@@ -23,12 +23,12 @@ public class Drive extends Command {
     private final SlewRateLimiter rotationLimiter;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
-                 Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
+                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID) {
         this.drivetrain = drivetrain;
         addRequirements(drivetrain);
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
-        this.rotationsSpeed = rotationsSpeed;
+        this.rotationSpeed = rotationSpeed;
         this.fieldRelative = fieldRelative;
         this.usePID = usePID;
         xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
@@ -40,7 +40,7 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
     public void execute() {
         double xSpeed = xLimiter.calculate(this.xSpeed.get());
         double ySpeed = yLimiter.calculate(this.ySpeed.get());
-        double rotationSpeed = rotationLimiter.calculate(this.rotationsSpeed.get());
+        double rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 

From 61bbb881548e3b0961cd0dd5bca93ef5b09c0e54 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:41:07 +0200
Subject: [PATCH 049/158] merged into 1 line

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 8195845..cba4d73 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -102,7 +102,7 @@ public void stop() {
     }
 
     public void set(SwerveModuleState state, boolean usePID) {
-        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED){
+        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED) {
             stop();
             return;
         }
@@ -123,10 +123,7 @@ private SwerveModuleState optimize(SwerveModuleState state, double currentAngle)
         while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
-            }
-            else {
-                desiredAngle += DEGREES_TO_FLIP;
-            }
+            } else {desiredAngle += DEGREES_TO_FLIP;}
             state.speedMetersPerSecond *= -1;
         }
         return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));

From 31d2100a9ea9bed612b461d19a8e5297c394ebe5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:43:45 +0200
Subject: [PATCH 050/158] accommodated minus

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index cba4d73..9bb4840 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -1,13 +1,9 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-import com.revrobotics.spark.config.EncoderConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
 import com.spikes2212.command.DashboardedSubsystem;
-import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.util.UnifiedControlMode;
@@ -16,6 +12,8 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
+import static java.lang.Math.abs;
+
 public class SwerveModule extends DashboardedSubsystem {
 
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
@@ -102,7 +100,7 @@ public void stop() {
     }
 
     public void set(SwerveModuleState state, boolean usePID) {
-        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED) {
+        if (Math.abs(state.speedMetersPerSecond) < Drivetrain.MIN_SPEED) {
             stop();
             return;
         }
@@ -120,7 +118,7 @@ private double normalizeAngleRelativeToEncoder(double currentAngle, double desir
 
     private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
         double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
-        while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
+        while (abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
             } else {desiredAngle += DEGREES_TO_FLIP;}

From eba8568fa46e360d87a5f3f7540a4f4f51231411 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:44:28 +0200
Subject: [PATCH 051/158] 0 default offset value

---
 .../java/frc/robot/subsystems/SwerveModuleHolder.java     | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 474e72a..0d07061 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -31,10 +31,10 @@ public class SwerveModuleHolder {
     private static final String BACK_LEFT_NAMESPACE_NAME = "back left";
     private static final String BACK_RIGHT_NAMESPACE_NAME = "back right";
 
-    private static final double FRONT_LEFT_OFFSET = -1;
-    private static final double FRONT_RIGHT_OFFSET = -1;
-    private static final double BACK_LEFT_OFFSET = -1;
-    private static final double BACK_RIGHT_OFFSET = -1;
+    private static final double FRONT_LEFT_OFFSET = 0;
+    private static final double FRONT_RIGHT_OFFSET = 0;
+    private static final double BACK_LEFT_OFFSET = 0;
+    private static final double BACK_RIGHT_OFFSET = 0;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
             new PIDSettings(0, 0, 0, 0, 0,0));

From 021b6f75fc257c84be3514485305b477d948c84a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:47:02 +0200
Subject: [PATCH 052/158] removed unnecessary values

---
 src/main/deploy/pathplanner/navgrid.json                   | 1 +
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 4 ++--
 2 files changed, 3 insertions(+), 2 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/navgrid.json

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 0d07061..b14e179 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -37,9 +37,9 @@ public class SwerveModuleHolder {
     private static final double BACK_RIGHT_OFFSET = 0;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(0, 0, 0, 0, 0,0));
+            new PIDSettings(0, 0, 0));
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(0, 0, 0, 0, 0,0));
+            new PIDSettings(0, 0, 0));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
             "drive", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(

From 906875acf1000e2baf3ae34106f5c8a3da4b7997 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:47:55 +0200
Subject: [PATCH 053/158] ditto

---
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index b14e179..ab05e88 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -41,9 +41,9 @@ public class SwerveModuleHolder {
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
             new PIDSettings(0, 0, 0));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "drive", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_VELOCITY));
+            "drive", new FeedForwardSettings(FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "turn", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_POSITION));
+            "turn", new FeedForwardSettings(FeedForwardController.ControlMode.LINEAR_POSITION));
 
     private static SwerveModule frontLeft;
     private static SwerveModule frontRight;

From 437c66522aeaedefa0a8e2614ab805c2d4e2954b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:10:07 +0200
Subject: [PATCH 054/158] unmerged

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 9bb4840..55809a1 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -121,7 +121,9 @@ private SwerveModuleState optimize(SwerveModuleState state, double currentAngle)
         while (abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
-            } else {desiredAngle += DEGREES_TO_FLIP;}
+            } else {
+                desiredAngle += DEGREES_TO_FLIP;
+            }
             state.speedMetersPerSecond *= -1;
         }
         return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));

From 9ff00ed7e84fec3ad097bc5522612f3996b94c93 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:11:14 +0200
Subject: [PATCH 055/158] deleted unnecessary imports

---
 src/main/java/frc/robot/subsystems/Drivetrain.java         | 1 -
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 2 --
 2 files changed, 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 3cc2747..a1af8ea 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.SerialPort;
 
 public class Drivetrain extends DashboardedSubsystem {
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index ab05e88..03c9632 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -1,9 +1,7 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.spark.SparkLowLevel;
-import com.revrobotics.spark.SparkMax;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;

From 971f2e2f57843ce9753f2151583a73189184b30a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:12:10 +0200
Subject: [PATCH 056/158] deleted weird file

---
 src/main/deploy/pathplanner/navgrid.json | 1 -
 1 file changed, 1 deletion(-)
 delete mode 100644 src/main/deploy/pathplanner/navgrid.json

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
deleted file mode 100644
index 23e0db9..0000000
--- a/src/main/deploy/pathplanner/navgrid.json
+++ /dev/null
@@ -1 +0,0 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From dbda02b68c2ad8b75616c4fbaab7338b1880d187 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:14:38 +0200
Subject: [PATCH 057/158] deleted unnecessary given values

---
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 03c9632..1266886 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -34,10 +34,8 @@ public class SwerveModuleHolder {
     private static final double BACK_LEFT_OFFSET = 0;
     private static final double BACK_RIGHT_OFFSET = 0;
 
-    private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(0, 0, 0));
-    private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(0, 0, 0));
+    private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive");
+    private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn");
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
             "drive", new FeedForwardSettings(FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(

From 2390dd1f51eddad16b2b301413a6c7e48043c0ee Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:37:19 +0200
Subject: [PATCH 058/158] add requirements FIRST (get it??? like FIRST the
 program, do you get it??)

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 9a03382..e8565a9 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -24,8 +24,8 @@ public class Drive extends Command {
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID) {
-        this.drivetrain = drivetrain;
         addRequirements(drivetrain);
+        this.drivetrain = drivetrain;
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationSpeed = rotationSpeed;

From 711b5f66b8b9884bc29fe5adfbb4b7f45a3f41df Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:40:06 +0200
Subject: [PATCH 059/158] changed units to meters

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index a1af8ea..40cc0ae 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,8 +13,8 @@ public class Drivetrain extends DashboardedSubsystem {
     public static final double MIN_SPEED = 0.05;
     public static final double MAX_TURN_SPEED = 3;
 
-    private static final double TRACK_WIDTH = 60;
-    private static final double TRACK_LENGTH = 60;
+    private static final double TRACK_WIDTH = 0.6;
+    private static final double TRACK_LENGTH = 0.6;
 
     private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     private static final Translation2d FRONT_LEFT_WHEEL_POSITION =

From d0fefec3efa59cb0082f938ce05f9d036d586830 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:41:20 +0200
Subject: [PATCH 060/158] called configureDashboard

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 40cc0ae..2e67e88 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -56,6 +56,7 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.gyro = gyro;
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        configureDashboard();
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,

From bde93e59d6248e9e8c4ad843981d47baf58effdd Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:42:32 +0200
Subject: [PATCH 061/158] ditto

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 55809a1..95796f8 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -59,6 +59,7 @@ public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper tu
         configureDriveController();
         configureTurnController();
         configureAbsoluteEncoder();
+        configureDashboard();
     }
 
     public void configureDriveController() {

From 8ec1fdac9960427fa9069bddeb93d80cd92b2fa4 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:43:50 +0200
Subject: [PATCH 062/158] boom.

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 95796f8..51e1517 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -12,8 +12,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
-import static java.lang.Math.abs;
-
 public class SwerveModule extends DashboardedSubsystem {
 
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
@@ -119,7 +117,7 @@ private double normalizeAngleRelativeToEncoder(double currentAngle, double desir
 
     private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
         double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
-        while (abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
+        while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
             } else {

From b00b68383d53859f96f27542409cc5fbdef90a6f Mon Sep 17 00:00:00 2001
From: TurtleMinecraft <schnitzelgirl69@gmail.com>
Date: Fri, 31 Jan 2025 18:09:16 +0200
Subject: [PATCH 063/158] updated libraries

---
 .idea/jarRepositories.xml                     |  5 ++
 ....2.1.json => PathplannerLib-2025.2.2.json} |  8 +-
 ...enix6-25.1.0.json => Phoenix6-25.2.1.json} | 84 +++++++++++++------
 ...VLib-2025.0.0.json => REVLib-2025.0.2json} | 15 ++--
 ...ca-2025.0.0.json => Studica-2025.0.1.json} | 14 ++--
 vendordeps/photonlib.json                     | 12 +--
 6 files changed, 85 insertions(+), 53 deletions(-)
 rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.2.json} (87%)
 rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.1.json} (85%)
 rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.2json} (86%)
 rename vendordeps/{Studica-2025.0.0.json => Studica-2025.0.1.json} (88%)

diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index 02e25a8..a105306 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -111,5 +111,10 @@
       <option name="name" value="WPIcb311d09-36e9-4143-a032-55bb2b94443b_0Release" />
       <option name="url" value="https://dev.studica.com/maven/release/2025/" />
     </remote-repository>
+    <remote-repository>
+      <option name="id" value="WPILocal" />
+      <option name="name" value="WPILocal" />
+      <option name="url" value="file:/$PROJECT_DIR$/../../../../../../Public/wpilib/2025/maven" />
+    </remote-repository>
   </component>
 </project>
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.2.1.json
rename to vendordeps/PathplannerLib-2025.2.2.json
index 731bf22..c0f7c11 100644
--- a/vendordeps/PathplannerLib-2025.2.1.json
+++ b/vendordeps/PathplannerLib-2025.2.2.json
@@ -1,7 +1,7 @@
 {
-  "fileName": "PathplannerLib-2025.2.1.json",
+  "fileName": "PathplannerLib.json",
   "name": "PathplannerLib",
-  "version": "2025.2.1",
+  "version": "2025.2.2",
   "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
   "frcYear": "2025",
   "mavenUrls": [
@@ -12,7 +12,7 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-java",
-      "version": "2025.2.1"
+      "version": "2025.2.2"
     }
   ],
   "jniDependencies": [],
@@ -20,7 +20,7 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-cpp",
-      "version": "2025.2.1",
+      "version": "2025.2.2",
       "libName": "PathplannerLib",
       "headerClassifier": "headers",
       "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.1.json
similarity index 85%
rename from vendordeps/Phoenix6-25.1.0.json
rename to vendordeps/Phoenix6-25.2.1.json
index 18462c6..349f124 100644
--- a/vendordeps/Phoenix6-25.1.0.json
+++ b/vendordeps/Phoenix6-25.2.1.json
@@ -1,7 +1,7 @@
 {
-  "fileName": "Phoenix6-25.1.0.json",
+  "fileName": "Phoenix6-25.2.1.json",
   "name": "CTRE-Phoenix (v6)",
-  "version": "25.1.0",
+  "version": "25.2.1",
   "frcYear": "2025",
   "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
   "mavenUrls": [
@@ -19,14 +19,14 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "wpiapi-java",
-      "version": "25.1.0"
+      "version": "25.2.1"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "api-cpp",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -40,7 +40,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "tools",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -54,7 +54,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "api-cpp-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -68,7 +68,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "tools-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -82,7 +82,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simTalonSRX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -96,7 +96,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simVictorSPX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -110,7 +110,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simPigeonIMU",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -124,7 +124,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simCANCoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -138,7 +138,21 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFX",
-      "version": "25.1.0",
+      "version": "25.2.1",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProTalonFXS",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -152,7 +166,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANcoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -166,7 +180,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProPigeon2",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -180,7 +194,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANrange",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -196,7 +210,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "wpiapi-cpp",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_Phoenix6_WPI",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -212,7 +226,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "tools",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_PhoenixTools",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -228,7 +242,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "wpiapi-cpp-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_Phoenix6_WPISim",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -244,7 +258,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "tools-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_PhoenixTools_Sim",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -260,7 +274,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simTalonSRX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimTalonSRX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -276,7 +290,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simVictorSPX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimVictorSPX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -292,7 +306,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simPigeonIMU",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimPigeonIMU",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -308,7 +322,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simCANCoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimCANCoder",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -324,7 +338,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProTalonFX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -337,10 +351,26 @@
       ],
       "simMode": "swsim"
     },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProTalonFXS",
+      "version": "25.2.1",
+      "libName": "CTRE_SimProTalonFXS",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANcoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProCANcoder",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -356,7 +386,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProPigeon2",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProPigeon2",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -372,7 +402,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANrange",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProCANrange",
       "headerClassifier": "headers",
       "sharedLibrary": true,
diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.2json
similarity index 86%
rename from vendordeps/REVLib-2025.0.0.json
rename to vendordeps/REVLib-2025.0.2json
index 1a4f3cd..30ea8de 100644
--- a/vendordeps/REVLib-2025.0.0.json
+++ b/vendordeps/REVLib-2025.0.2json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.0.json",
+    "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json
similarity index 88%
rename from vendordeps/Studica-2025.0.0.json
rename to vendordeps/Studica-2025.0.1.json
index 8cba530..a3ed1fe 100644
--- a/vendordeps/Studica-2025.0.0.json
+++ b/vendordeps/Studica-2025.0.1.json
@@ -1,13 +1,13 @@
 {
-  "fileName": "Studica-2025.0.0.json",
+  "fileName": "Studica-2025.0.1.json",
   "name": "Studica",
-  "version": "2025.0.0",
+  "version": "2025.0.1",
   "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
   "frcYear": "2025",
   "mavenUrls": [
     "https://dev.studica.com/maven/release/2025/"
   ],
-  "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json",
+  "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
   "cppDependencies": [
     {
       "artifactId": "Studica-cpp",
@@ -24,7 +24,7 @@
       "libName": "Studica",
       "sharedLibrary": false,
       "skipInvalidPlatforms": true,
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     },
     {
       "artifactId": "Studica-driver",
@@ -41,14 +41,14 @@
       "libName": "StudicaDriver",
       "sharedLibrary": false,
       "skipInvalidPlatforms": true,
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     }
   ],
   "javaDependencies": [
     {
       "artifactId": "Studica-java",
       "groupId": "com.studica.frc",
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     }
   ],
   "jniDependencies": [
@@ -65,7 +65,7 @@
         "osxuniversal",
         "windowsx86-64"
       ],
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     }
   ]
 }
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 1163429..a908923 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2025.0.0-beta-8",
+  "version": "v2025.1.1",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "frcYear": "2025",
   "mavenUrls": [
@@ -13,7 +13,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2025.0.0-beta-8",
+      "version": "v2025.1.1",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -28,7 +28,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2025.0.0-beta-8",
+      "version": "v2025.1.1",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -43,7 +43,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2025.0.0-beta-8",
+      "version": "v2025.1.1",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -60,12 +60,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2025.0.0-beta-8"
+      "version": "v2025.1.1"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2025.0.0-beta-8"
+      "version": "v2025.1.1"
     }
   ]
 }

From 13d3635cabd15ba0b41fc89bc48ac3042802a176 Mon Sep 17 00:00:00 2001
From: TurtleMinecraft <schnitzelgirl69@gmail.com>
Date: Fri, 31 Jan 2025 18:11:06 +0200
Subject: [PATCH 064/158] bruh

---
 vendordeps/{REVLib-2025.0.2json => REVLib-2025.0.2.json} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename vendordeps/{REVLib-2025.0.2json => REVLib-2025.0.2.json} (100%)

diff --git a/vendordeps/REVLib-2025.0.2json b/vendordeps/REVLib-2025.0.2.json
similarity index 100%
rename from vendordeps/REVLib-2025.0.2json
rename to vendordeps/REVLib-2025.0.2.json

From d70f94186d14cfc5bfc5e983ee47778a0b0a3b27 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 18:37:39 +0200
Subject: [PATCH 065/158] not done yet

---
 src/main/java/frc/robot/RobotMap.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 3f149cc..2b2e462 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -4,17 +4,17 @@ public class RobotMap {
 
     public interface CAN {
 
-        int FRONT_LEFT_DRIVE_TALON_FX = -1;
-        int FRONT_RIGHT_DRIVE_TALON_FX = -1;
+        int FRONT_LEFT_DRIVE_TALON_FX = 1;
+        int FRONT_RIGHT_DRIVE_TALON_FX = 4;
         int BACK_LEFT_DRIVE_TALON_FX = -1;
         int BACK_RIGHT_DRIVE_TALON_FX = -1;
 
-        int FRONT_LEFT_TURN_SPARK_MAX = -1;
+        int FRONT_LEFT_TURN_SPARK_MAX = 2;
         int FRONT_RIGHT_TURN_SPARK_MAX = -1;
         int BACK_LEFT_TURN_SPARK_MAX = -1;
         int BACK_RIGHT_TURN_SPARK_MAX = -1;
 
-        int FRONT_LEFT_ABSOLUTE_ENCODER = -1;
+        int FRONT_LEFT_ABSOLUTE_ENCODER = 3;
         int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
         int BACK_LEFT_ABSOLUTE_ENCODER = -1;
         int BACK_RIGHT_ABSOLUTE_ENCODER = -1;

From 98a6e0fa520e443baf53315ab13098be7795253a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 18:46:13 +0200
Subject: [PATCH 066/158] updated CAN ports

---
 src/main/java/frc/robot/RobotMap.java | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 2b2e462..9b94f82 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -6,18 +6,18 @@ public interface CAN {
 
         int FRONT_LEFT_DRIVE_TALON_FX = 1;
         int FRONT_RIGHT_DRIVE_TALON_FX = 4;
-        int BACK_LEFT_DRIVE_TALON_FX = -1;
-        int BACK_RIGHT_DRIVE_TALON_FX = -1;
+        int BACK_LEFT_DRIVE_TALON_FX = 7;
+        int BACK_RIGHT_DRIVE_TALON_FX = 10;
 
         int FRONT_LEFT_TURN_SPARK_MAX = 2;
-        int FRONT_RIGHT_TURN_SPARK_MAX = -1;
-        int BACK_LEFT_TURN_SPARK_MAX = -1;
-        int BACK_RIGHT_TURN_SPARK_MAX = -1;
+        int FRONT_RIGHT_TURN_SPARK_MAX = 5;
+        int BACK_LEFT_TURN_SPARK_MAX = 8;
+        int BACK_RIGHT_TURN_SPARK_MAX = 11;
 
         int FRONT_LEFT_ABSOLUTE_ENCODER = 3;
-        int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
-        int BACK_LEFT_ABSOLUTE_ENCODER = -1;
-        int BACK_RIGHT_ABSOLUTE_ENCODER = -1;
+        int FRONT_RIGHT_ABSOLUTE_ENCODER = 6;
+        int BACK_LEFT_ABSOLUTE_ENCODER = 9;
+        int BACK_RIGHT_ABSOLUTE_ENCODER = 12;
     }
     
     public interface DIO {

From 55b057c14fd442a6caeb04a9b63dcb453f52c0e9 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 18:48:59 +0200
Subject: [PATCH 067/158] added reset gyro func

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 2e67e88..9b02a28 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -91,6 +91,10 @@ public void resetRelativeEncoders() {
         backRight.resetRelativeEncoder();
     }
 
+    public void resetGyro() {
+        gyro.reset();
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);

From f0adb7e641350e60b8b46cbda69ad969e5645af2 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 19:03:04 +0200
Subject: [PATCH 068/158] added option to disable acceleration

---
 src/main/java/frc/robot/commands/Drive.java | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index e8565a9..476aec5 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -21,16 +21,19 @@ public class Drive extends Command {
     private final SlewRateLimiter xLimiter;
     private final SlewRateLimiter yLimiter;
     private final SlewRateLimiter rotationLimiter;
+    private final boolean limitAcceleration;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
-                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID) {
+                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,
+                 boolean limitAcceleration) {
         addRequirements(drivetrain);
         this.drivetrain = drivetrain;
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationSpeed = rotationSpeed;
-        this.fieldRelative = fieldRelative;
         this.usePID = usePID;
+        this.limitAcceleration = limitAcceleration;
+        this.fieldRelative = fieldRelative;
         xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
@@ -38,9 +41,16 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
 
     @Override
     public void execute() {
-        double xSpeed = xLimiter.calculate(this.xSpeed.get());
-        double ySpeed = yLimiter.calculate(this.ySpeed.get());
-        double rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
+        double xSpeed = this.xSpeed.get();
+        double ySpeed = this.ySpeed.get();
+        double rotationSpeed = this.rotationSpeed.get();
+
+        if (limitAcceleration) {
+            xSpeed = xLimiter.calculate(this.xSpeed.get());
+            ySpeed = yLimiter.calculate(this.ySpeed.get());
+            rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
+        }
+
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 

From 60b7981d60fc4311abae244786da59fdec7a8b95 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 19:04:59 +0200
Subject: [PATCH 069/158] updated SpikesLib2.jsonl

---
 vendordeps/SpikesLib2.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
index bc10370..1719631 100644
--- a/vendordeps/SpikesLib2.json
+++ b/vendordeps/SpikesLib2.json
@@ -15,7 +15,7 @@
     {
       "groupId": "com.github.Spikes-2212-Programming-Guild",
       "artifactId": "SpikesLib2",
-      "version": "v5.1.0"
+      "version": "v5.1.1"
     }
   ]
 }

From 4086d1b9f7efcd594333fd74644ae095f62a8cd2 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:28:47 +0200
Subject: [PATCH 070/158] updated SpikesLib2.json

---
 vendordeps/SpikesLib2.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
index 1719631..7397e86 100644
--- a/vendordeps/SpikesLib2.json
+++ b/vendordeps/SpikesLib2.json
@@ -1,7 +1,7 @@
 {
   "fileName": "SpikesLib2.json",
   "name": "SpikesLib2",
-  "version": "v5.1.0",
+  "version": "v5.1.1",
   "frcYear": "2025",
   "uuid": "018a8f54-8662-11ec-a8a3-0242ac120002",
   "jsonUrl": "https://raw.githubusercontent.com/Spikes-2212-Programming-Guild/SpikesLib2/master/SpikesLib2.json",

From 7ca4f37f910142320ce6ba8a7318b992b27fd53b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:46:05 +0200
Subject: [PATCH 071/158] added override to drive command end func

---
 src/main/java/frc/robot/commands/Drive.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 476aec5..c5c534b 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -54,6 +54,7 @@ public void execute() {
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 
+    @Override
     public void end(boolean interrupted) {
         drivetrain.stop();
     }

From 912f2117b30e669c730440e187032f9bdc75b817 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:48:06 +0200
Subject: [PATCH 072/158] limitAcceleration below usePID

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index c5c534b..630be5d 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -17,11 +17,11 @@ public class Drive extends Command {
     private final Supplier<Double> rotationSpeed;
     private final boolean fieldRelative;
     private final boolean usePID;
+    private final boolean limitAcceleration;
 
     private final SlewRateLimiter xLimiter;
     private final SlewRateLimiter yLimiter;
     private final SlewRateLimiter rotationLimiter;
-    private final boolean limitAcceleration;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,

From 8f7e3673bca5540406a16176f81bd8890a4ba759 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:49:44 +0200
Subject: [PATCH 073/158] initializing the values in the same order as received

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 630be5d..f394ebf 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -31,9 +31,9 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationSpeed = rotationSpeed;
+        this.fieldRelative = fieldRelative;
         this.usePID = usePID;
         this.limitAcceleration = limitAcceleration;
-        this.fieldRelative = fieldRelative;
         xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);

From b39317d512df334616879dbb729fea924e3ab008 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:50:25 +0200
Subject: [PATCH 074/158] added blank line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9b02a28..f010694 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -33,6 +33,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveModule backLeft;
     private final SwerveModule backRight;
     private final AHRS gyro;
+
     private final SwerveDriveKinematics kinematics;
 
     private static Drivetrain instance;

From f2754a4039bdd4bd02208b9b15fbd3bf5b2c5daa Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 10:30:26 +0200
Subject: [PATCH 075/158] added odometry

---
 .../java/frc/robot/subsystems/Drivetrain.java | 22 +++++++++++++++----
 .../frc/robot/subsystems/SwerveModule.java    |  6 +++++
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index f010694..b185b2f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,10 +2,9 @@
 
 import com.spikes2212.command.DashboardedSubsystem;
 import com.studica.frc.AHRS;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.kinematics.*;
 
 public class Drivetrain extends DashboardedSubsystem {
 
@@ -34,8 +33,11 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveModule backRight;
     private final AHRS gyro;
 
+    private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
+    private static Pose2d pose2d;
+
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {
@@ -48,15 +50,19 @@ public static Drivetrain getInstance() {
     }
 
     private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
-                      SwerveModule backRight, AHRS gyro) {
+                       SwerveModule backRight, AHRS gyro) {
         super(NAMESPACE_NAME);
         this.frontLeft = frontLeft;
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
         this.gyro = gyro;
+        this.pose2d = new Pose2d();
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
+                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
+                 backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()});
         configureDashboard();
     }
 
@@ -96,6 +102,14 @@ public void resetGyro() {
         gyro.reset();
     }
 
+    @Override
+    public void periodic() {
+        pose2d = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
+                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
+                backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()
+        });
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 51e1517..10ba995 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -9,7 +9,9 @@
 import com.spikes2212.util.UnifiedControlMode;
 import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
 import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
 public class SwerveModule extends DashboardedSubsystem {
@@ -136,6 +138,10 @@ public void resetRelativeEncoder() {
         turnMotor.setPosition(getAbsoluteAngle());
     }
 
+    public SwerveModulePosition getSwerveModulePosition() {
+        return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(turnMotor.getPosition()));
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);

From b881427a1d1aee7e72fb16da258ea61587d2022f Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:15:25 +0200
Subject: [PATCH 076/158] renamed currentPose and removed this

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index b185b2f..fb43d7f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -36,7 +36,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
-    private static Pose2d pose2d;
+    private static Pose2d currentPose;
 
     private static Drivetrain instance;
 
@@ -57,7 +57,7 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.backLeft = backLeft;
         this.backRight = backRight;
         this.gyro = gyro;
-        this.pose2d = new Pose2d();
+        currentPose = new Pose2d();
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
@@ -104,7 +104,7 @@ public void resetGyro() {
 
     @Override
     public void periodic() {
-        pose2d = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
+        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
                 frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
                 backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()
         });

From 5f6eda107eb518bcff2fe649c17991335bac9fde Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:16:43 +0200
Subject: [PATCH 077/158] renamed getModulePosition

---
 src/main/java/frc/robot/subsystems/Drivetrain.java   | 8 ++++----
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index fb43d7f..9c94d8b 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -61,8 +61,8 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
-                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
-                 backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()});
+                frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                 backLeft.getModulePosition(), backRight.getModulePosition()});
         configureDashboard();
     }
 
@@ -105,8 +105,8 @@ public void resetGyro() {
     @Override
     public void periodic() {
         currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
-                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
-                backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()
+                frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                backLeft.getModulePosition(), backRight.getModulePosition()
         });
     }
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 10ba995..6c35888 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -138,7 +138,7 @@ public void resetRelativeEncoder() {
         turnMotor.setPosition(getAbsoluteAngle());
     }
 
-    public SwerveModulePosition getSwerveModulePosition() {
+    public SwerveModulePosition getModulePosition() {
         return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(turnMotor.getPosition()));
     }
 

From 77e7d06b92019ff21c3a011cf8706ec37cd9b654 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:48:56 +0200
Subject: [PATCH 078/158] currentPose not static

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9c94d8b..6de8dda 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -36,7 +36,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
-    private static Pose2d currentPose;
+    private Pose2d currentPose;
 
     private static Drivetrain instance;
 

From 282616dd6686c8eaedcd5c74e5752555aa5c7d51 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:49:46 +0200
Subject: [PATCH 079/158] periodic below constructor

---
 .../java/frc/robot/subsystems/Drivetrain.java    | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 6de8dda..2416e04 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -66,6 +66,14 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         configureDashboard();
     }
 
+    @Override
+    public void periodic() {
+        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
+                frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                backLeft.getModulePosition(), backRight.getModulePosition()
+        });
+    }
+
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
                       boolean usePID) {
         ChassisSpeeds speeds;
@@ -102,14 +110,6 @@ public void resetGyro() {
         gyro.reset();
     }
 
-    @Override
-    public void periodic() {
-        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
-                frontLeft.getModulePosition(), frontRight.getModulePosition(),
-                backLeft.getModulePosition(), backRight.getModulePosition()
-        });
-    }
-
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);

From f920359d566650f5d366c6bacd08398cee4ce6eb Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:06:34 +0200
Subject: [PATCH 080/158] removed blank line

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 53b39b2..6a069d5 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -46,7 +46,6 @@ public class SwerveModule extends DashboardedSubsystem {
     private final boolean driveInverted;
     private final double offset;
 
-
     private final PIDSettings drivePIDSettings;
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;

From 6ccb6e47a59c85b86aecc5df4de1de1fef652f88 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:09:05 +0200
Subject: [PATCH 081/158] separated the turnEncoder from the control loop
 related fields

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 6a069d5..2df487b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -51,6 +51,7 @@ public class SwerveModule extends DashboardedSubsystem {
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
     private final FeedForwardController turnFeedForwardController;
+
     private final RelativeEncoder turnEncoder;
 
     private final EncoderConfig turnEncoderConfig;

From 1e59a77516bb039b741927e7df65428637667ec2 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 12:43:31 +0200
Subject: [PATCH 082/158] added a todo to check if new AHRS is correct as well
 as adding gyro angle to the dashboard

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index adad7e2..0255c67 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -49,6 +49,7 @@ public static Drivetrain getInstance() {
         if (instance == null) {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
+                    //TODO check in shuffleboard if this is correct
                     new AHRS(AHRS.NavXComType.kI2C));
         }
         return instance;
@@ -163,5 +164,6 @@ public void driveRobotRelative(ChassisSpeeds speeds) {
 
     @Override
     public void configureDashboard() {
+        namespace.putNumber("gyro", gyro::getAngle);
     }
 }

From d01cefd55056a087b44a36373e5593dd205db3d3 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 24 Jan 2025 00:21:23 +0200
Subject: [PATCH 083/158] committing in order to change branch

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 2df487b..52b3334 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -74,8 +74,7 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
-        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings,
-                FeedForwardController.DEFAULT_PERIOD);
+        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings);
         this.turnEncoder = turnMotor.getEncoder();
         this.sparkConfig = new SparkMaxConfig();
         MotorOutput = new MotorOutputConfigs();
@@ -137,7 +136,7 @@ private void setSpeed(double speed, boolean usePID) {
     private void setAngle(double angle) {
         configureTurnController();
         turnConfigureFF();
-        double feedForward = turnFeedForwardController.calculate(angle);
+        double feedForward = turnFeedForwardController.calculate( 0,angle);
         turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity, ClosedLoopSlot.kSlot0,
                 feedForward, SparkClosedLoopController.ArbFFUnits.kPercentOut);
     }

From 77fb366c03c3941427132b9cb342cf1c86cc18fa Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:50:06 +0200
Subject: [PATCH 084/158] deleted extra line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 0255c67..23eb698 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -33,7 +33,6 @@ public class Drivetrain extends DashboardedSubsystem {
 
     private static final String NAMESPACE_NAME = "drivetrain";
 
-
     private final SwerveModule frontLeft;
     private final SwerveModule frontRight;
     private final SwerveModule backLeft;

From 287529517f8c06799917a98be20a3d0ed0a292e2 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:50:58 +0200
Subject: [PATCH 085/158] changed navx port

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 23eb698..69a467c 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -49,7 +49,7 @@ public static Drivetrain getInstance() {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
                     //TODO check in shuffleboard if this is correct
-                    new AHRS(AHRS.NavXComType.kI2C));
+                    new AHRS(AHRS.NavXComType.kMXP_SPI));
         }
         return instance;
     }

From d1448f873b6a5d060e77931e517aaac780346a7e Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:51:57 +0200
Subject: [PATCH 086/158] changed gear ratio from drive to turn

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 52b3334..71f5afb 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -105,7 +105,7 @@ public void configureDriveController() {
     }
 
     public void configureTurnController() {
-        turnEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnEncoderConfig.positionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
         turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
         ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
         turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());

From 083e781ebf858b20d015b3e789914b8579612fee Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:59:17 +0200
Subject: [PATCH 087/158] updated SpikesLib2.json

---
 build.gradle               |  1 -
 vendordeps/SpikesLib2.json | 21 +++++++++++++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)
 create mode 100644 vendordeps/SpikesLib2.json

diff --git a/build.gradle b/build.gradle
index 1345de4..2e6cdec 100644
--- a/build.gradle
+++ b/build.gradle
@@ -85,7 +85,6 @@ dependencies {
     implementation group: 'com.ctre.phoenix6', name: 'wpiapi-java', version: '24.0.0-beta-8'
     implementation group: 'com.ctre.phoenix', name: 'api-java', version: '5.32.0-beta-5'
     implementation group: 'com.ctre.phoenix', name: 'wpiapi-java', version: '5.31.0'
-    implementation 'com.github.Spikes-2212-Programming-Guild:SpikesLib2:update-wpilib-25-SNAPSHOT'
 }
 
 test {
diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
new file mode 100644
index 0000000..f04853c
--- /dev/null
+++ b/vendordeps/SpikesLib2.json
@@ -0,0 +1,21 @@
+{
+  "fileName": "SpikesLib2.json",
+  "name": "SpikesLib2",
+  "version": "v5.1.0",
+  "frcYear": "2025",
+  "uuid": "018a8f54-8662-11ec-a8a3-0242ac120002",
+  "jsonUrl": "https://raw.githubusercontent.com/Spikes-2212-Programming-Guild/SpikesLib2/master/SpikesLib2.json",
+  "mavenUrls": [
+    "https://jitpack.io"
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "com.github.Spikes-2212-Programming-Guild",
+      "artifactId": "SpikesLib2",
+      "version": "v5.1.0"
+    }
+  ]
+}
\ No newline at end of file

From 7fc8d5fed0863e472bff1e8ad715575f13c739ae Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:00:18 +0200
Subject: [PATCH 088/158] added wait correct parameters

---
 .../java/frc/robot/subsystems/SwerveModuleHolder.java    | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index ed6fc7f..2731a9c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -4,6 +4,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.spark.SparkLowLevel;
 import com.revrobotics.spark.SparkMax;
+import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.dashboard.RootNamespace;
@@ -34,13 +35,13 @@ public class SwerveModuleHolder {
     private static final double BACK_RIGHT_OFFSET = -1;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(-1, -1, -1, -1, -1));
+            new PIDSettings(-1, -1, -1, -1, -1, -1));
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(-1, -1, -1, -1, -1));
+            new PIDSettings(-1, -1, -1, -1, -1, -1));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "drive", new FeedForwardSettings(-1, -1, -1, -1));
+            "drive", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "turn", new FeedForwardSettings(-1, -1, -1, -1));
+            "turn", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_POSITION));
 
     private static SwerveModule frontLeft;
     private static SwerveModule frontRight;

From a274b75685954e240e33674ed4c9ae34f490a55d Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:34:13 +0200
Subject: [PATCH 089/158] replaced motor controllers with wrappers

---
 .../deploy/pathplanner/autos/algae auto.auto  |  87 ++++++++++++++
 .../deploy/pathplanner/autos/best auto.auto   | 106 ++++++++++++++++++
 .../pathplanner/autos/just rp auto.auto       |  44 ++++++++
 .../pathplanner/autos/mirror best auto.auto   | 106 ++++++++++++++++++
 .../pathplanner/paths/algae path pt1.path     |  54 +++++++++
 .../pathplanner/paths/algae path pt2.path     |  54 +++++++++
 .../pathplanner/paths/algae path pt3.path     |  54 +++++++++
 .../pathplanner/paths/best path pt1.path      |  54 +++++++++
 .../pathplanner/paths/best path pt2.path      |  54 +++++++++
 .../pathplanner/paths/best path pt3.path      |  54 +++++++++
 .../pathplanner/paths/just rp path.path       |  54 +++++++++
 .../paths/mirror best path pt1.path           |  54 +++++++++
 .../paths/mirror best path pt2.path           |  54 +++++++++
 .../paths/mirror best path pt3.path           |  54 +++++++++
 .../robot/subsystems/SwerveModuleHolder.java  |  18 +--
 15 files changed, 893 insertions(+), 8 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/algae auto.auto
 create mode 100644 src/main/deploy/pathplanner/autos/best auto.auto
 create mode 100644 src/main/deploy/pathplanner/autos/just rp auto.auto
 create mode 100644 src/main/deploy/pathplanner/autos/mirror best auto.auto
 create mode 100644 src/main/deploy/pathplanner/paths/algae path pt1.path
 create mode 100644 src/main/deploy/pathplanner/paths/algae path pt2.path
 create mode 100644 src/main/deploy/pathplanner/paths/algae path pt3.path
 create mode 100644 src/main/deploy/pathplanner/paths/best path pt1.path
 create mode 100644 src/main/deploy/pathplanner/paths/best path pt2.path
 create mode 100644 src/main/deploy/pathplanner/paths/best path pt3.path
 create mode 100644 src/main/deploy/pathplanner/paths/just rp path.path
 create mode 100644 src/main/deploy/pathplanner/paths/mirror best path pt1.path
 create mode 100644 src/main/deploy/pathplanner/paths/mirror best path pt2.path
 create mode 100644 src/main/deploy/pathplanner/paths/mirror best path pt3.path

diff --git a/src/main/deploy/pathplanner/autos/algae auto.auto b/src/main/deploy/pathplanner/autos/algae auto.auto
new file mode 100644
index 0000000..9bbef9d
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/algae auto.auto	
@@ -0,0 +1,87 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "algae path pt1"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoralAngle"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL4"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ReleaseCoral"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "algae path pt2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeAlgaeAngle"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL3"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "TakeAlgae"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "algae path pt3"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "PlaceAngle"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/best auto.auto b/src/main/deploy/pathplanner/autos/best auto.auto
new file mode 100644
index 0000000..fbf7ad7
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/best auto.auto	
@@ -0,0 +1,106 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "best path pt1"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL4"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoralAngle"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ReleaseCoral"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "best path pt2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToFeeder"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeCoralAngle"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeCoral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "best path pt3"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL4"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoralAngle"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ReleaseCoral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/just rp auto.auto b/src/main/deploy/pathplanner/autos/just rp auto.auto
new file mode 100644
index 0000000..1fcd190
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/just rp auto.auto	
@@ -0,0 +1,44 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "just rp path"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoralAngle"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL4"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ReleaseCoral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/mirror best auto.auto b/src/main/deploy/pathplanner/autos/mirror best auto.auto
new file mode 100644
index 0000000..05f74ad
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/mirror best auto.auto	
@@ -0,0 +1,106 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "mirror best path pt1"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL4"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoralAngle"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ReleaseCoral"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "mirror best path pt2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToFeeder"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeCoralAngle"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeCoral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "mirror best path pt3"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ElevateToL4"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "OuttakeCoralAngle"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ReleaseCoral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/algae path pt1.path b/src/main/deploy/pathplanner/paths/algae path pt1.path
new file mode 100644
index 0000000..bdb0eab
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/algae path pt1.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.760204081632654,
+        "y": 3.980229591836734
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.732171786287433,
+        "y": 3.9788909963726784
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.887998102466793,
+        "y": 4.177751423149904
+      },
+      "prevControl": {
+        "x": 6.580665241888563,
+        "y": 4.172542262608721
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "coral 4"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "algae paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/algae path pt2.path b/src/main/deploy/pathplanner/paths/algae path pt2.path
new file mode 100644
index 0000000..521b6c5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/algae path pt2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.887998102466793,
+        "y": 4.177751423149904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.130205894281468,
+        "y": 4.115820933209493
+      },
+      "isLocked": false,
+      "linkedName": "coral 4"
+    },
+    {
+      "anchor": {
+        "x": 5.887308673469388,
+        "y": 3.980229591836734
+      },
+      "prevControl": {
+        "x": 5.610293015621114,
+        "y": 4.021834023918175
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "algae 4"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "algae paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/algae path pt3.path b/src/main/deploy/pathplanner/paths/algae path pt3.path
new file mode 100644
index 0000000..7fa558a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/algae path pt3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.887308673469388,
+        "y": 3.980229591836734
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.010199240986717,
+        "y": 3.5132827324478164
+      },
+      "isLocked": false,
+      "linkedName": "algae 4"
+    },
+    {
+      "anchor": {
+        "x": 6.346252371916509,
+        "y": 0.5804554079696379
+      },
+      "prevControl": {
+        "x": 6.203076223754949,
+        "y": 0.7853958641944134
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "algae paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/best path pt1.path b/src/main/deploy/pathplanner/paths/best path pt1.path
new file mode 100644
index 0000000..aba8568
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/best path pt1.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.834821428571429,
+        "y": 1.8909438775510194
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.3345723757534484,
+        "y": 2.050238162711536
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.391555977229602,
+        "y": 2.9099146110056915
+      },
+      "prevControl": {
+        "x": 6.152652916005112,
+        "y": 2.521904406924059
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "coral 5"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": 123.42775135269798
+  },
+  "reversed": false,
+  "folder": "best paths",
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -179.1574757392596
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/best path pt2.path b/src/main/deploy/pathplanner/paths/best path pt2.path
new file mode 100644
index 0000000..9f02d90
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/best path pt2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.391555977229602,
+        "y": 2.9099146110056915
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.4322291565293552,
+        "y": 2.0446787360764906
+      },
+      "isLocked": false,
+      "linkedName": "coral 5"
+    },
+    {
+      "anchor": {
+        "x": 1.2162627551020408,
+        "y": 1.107461734693877
+      },
+      "prevControl": {
+        "x": 2.479311766934477,
+        "y": 1.6268423441921782
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "get coral right"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -128.29016319224309
+  },
+  "reversed": false,
+  "folder": "best paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 123.42775135269798
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/best path pt3.path b/src/main/deploy/pathplanner/paths/best path pt3.path
new file mode 100644
index 0000000..c0dea53
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/best path pt3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2162627551020408,
+        "y": 1.107461734693877
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1340561224489796,
+        "y": 1.711862244897958
+      },
+      "isLocked": false,
+      "linkedName": "get coral right"
+    },
+    {
+      "anchor": {
+        "x": 3.627277039848197,
+        "y": 2.8488140417457295
+      },
+      "prevControl": {
+        "x": 2.7841010194400337,
+        "y": 2.1324875111334833
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 59.53445508054016
+  },
+  "reversed": false,
+  "folder": "best paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -128.29016319224309
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/just rp path.path b/src/main/deploy/pathplanner/paths/just rp path.path
new file mode 100644
index 0000000..cde3a4c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/just rp path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.760204081632654,
+        "y": 3.980229591836734
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.732171786287433,
+        "y": 3.9788909963726784
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.895635673624289,
+        "y": 3.841698292220113
+      },
+      "prevControl": {
+        "x": 6.5883028130460595,
+        "y": 3.8364891316789302
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "just rp paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/mirror best path pt1.path b/src/main/deploy/pathplanner/paths/mirror best path pt1.path
new file mode 100644
index 0000000..c127582
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/mirror best path pt1.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.715433673469388,
+        "y": 6.106823979591836
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.160471339250096,
+        "y": 5.922794993492372
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.376280834914611,
+        "y": 5.185910815939278
+      },
+      "prevControl": {
+        "x": 5.7283283432200225,
+        "y": 5.322400417362067
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "coral 3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": -122.82854179141262
+  },
+  "reversed": false,
+  "folder": "mirror best paths",
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -179.28384005452952
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/mirror best path pt2.path b/src/main/deploy/pathplanner/paths/mirror best path pt2.path
new file mode 100644
index 0000000..8ba2d3d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/mirror best path pt2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.376280834914611,
+        "y": 5.185910815939278
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0185818483085867,
+        "y": 6.167098172914402
+      },
+      "isLocked": false,
+      "linkedName": "coral 3"
+    },
+    {
+      "anchor": {
+        "x": 1.081951530612245,
+        "y": 6.957461734693877
+      },
+      "prevControl": {
+        "x": 1.5800877020775692,
+        "y": 6.761104869824657
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "get coral left"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 125.71995431229035
+  },
+  "reversed": false,
+  "folder": "mirror best paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -122.82854179141262
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/mirror best path pt3.path b/src/main/deploy/pathplanner/paths/mirror best path pt3.path
new file mode 100644
index 0000000..bb278e5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/mirror best path pt3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.081951530612245,
+        "y": 6.957461734693877
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.13405612244898,
+        "y": 6.2933673469387745
+      },
+      "isLocked": false,
+      "linkedName": "get coral left"
+    },
+    {
+      "anchor": {
+        "x": 3.6654648956356732,
+        "y": 5.147722960151802
+      },
+      "prevControl": {
+        "x": 2.5984368344111832,
+        "y": 5.819279082600782
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.85861444792467
+  },
+  "reversed": false,
+  "folder": "mirror best paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 125.71995431229035
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 2731a9c..ae328f1 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -8,6 +8,8 @@
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.dashboard.RootNamespace;
+import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
 import frc.robot.RobotMap;
 
 public class SwerveModuleHolder {
@@ -50,8 +52,8 @@ public class SwerveModuleHolder {
 
     public static SwerveModule getFrontLeft() {
         if (frontLeft == null) {
-            frontLeft = new SwerveModule(FRONT_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_LEFT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.FRONT_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            frontLeft = new SwerveModule(FRONT_LEFT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.FRONT_LEFT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.FRONT_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.FRONT_LEFT_ABSOLUTE_ENCODER),
                     FRONT_LEFT_CANCODER_INVERTED, FRONT_LEFT_DRIVE_INVERTED, FRONT_LEFT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
@@ -61,8 +63,8 @@ public static SwerveModule getFrontLeft() {
 
     public static SwerveModule getFrontRight() {
         if (frontRight == null) {
-            frontRight = new SwerveModule(FRONT_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.FRONT_RIGHT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.FRONT_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            frontRight = new SwerveModule(FRONT_RIGHT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.FRONT_RIGHT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.FRONT_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.FRONT_RIGHT_ABSOLUTE_ENCODER),
                     FRONT_RIGHT_CANCODER_INVERTED, FRONT_RIGHT_DRIVE_INVERTED, FRONT_RIGHT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
@@ -72,8 +74,8 @@ public static SwerveModule getFrontRight() {
 
     public static SwerveModule getBackLeft() {
         if (backLeft == null) {
-            backLeft = new SwerveModule(BACK_LEFT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_LEFT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.BACK_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            backLeft = new SwerveModule(BACK_LEFT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.BACK_LEFT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.BACK_LEFT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.BACK_LEFT_ABSOLUTE_ENCODER),
                     BACK_LEFT_CANCODER_INVERTED, BACK_LEFT_DRIVE_INVERTED, BACK_LEFT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);
@@ -83,8 +85,8 @@ public static SwerveModule getBackLeft() {
 
     public static SwerveModule getBackRight() {
         if (backRight == null) {
-            backRight = new SwerveModule(BACK_RIGHT_NAMESPACE_NAME, new TalonFX(RobotMap.CAN.BACK_RIGHT_DRIVE_TALON_FX),
-                    new SparkMax(RobotMap.CAN.BACK_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
+            backRight = new SwerveModule(BACK_RIGHT_NAMESPACE_NAME, new TalonFXWrapper(RobotMap.CAN.BACK_RIGHT_DRIVE_TALON_FX),
+                    SparkWrapper.createSparkMax(RobotMap.CAN.BACK_RIGHT_TURN_SPARK_MAX, SparkLowLevel.MotorType.kBrushless),
                     new CANcoder(RobotMap.CAN.BACK_RIGHT_ABSOLUTE_ENCODER),
                     BACK_RIGHT_CANCODER_INVERTED, BACK_RIGHT_DRIVE_INVERTED, BACK_RIGHT_OFFSET, drivePIDSettings,
                     turnPIDSettings, driveFeedForwardSettings, turnFeedForwardSettings);

From 524e6d9de4bc5e22b9f60e802306bbbb288e9e4c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 11:44:22 +0200
Subject: [PATCH 090/158] weh

---
 .../frc/robot/subsystems/SwerveModule.java    | 90 +++++--------------
 1 file changed, 23 insertions(+), 67 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 71f5afb..8aa059c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -2,27 +2,18 @@
 
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.spikes2212.command.DashboardedSubsystem;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
+import com.spikes2212.util.UnifiedControlMode;
+import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
 public class SwerveModule extends DashboardedSubsystem {
@@ -38,8 +29,8 @@ public class SwerveModule extends DashboardedSubsystem {
     private static final double DEGREES_TO_FLIP = 180;
     private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
 
-    private final TalonFX driveMotor;
-    private final SparkMax turnMotor;
+    private final TalonFXWrapper driveMotor;
+    private final SparkWrapper turnMotor;
     private final CANcoder absoluteEncoder;
 
     private final boolean cancoderInverted;
@@ -52,13 +43,11 @@ public class SwerveModule extends DashboardedSubsystem {
     private final FeedForwardSettings turnFeedForwardSettings;
     private final FeedForwardController turnFeedForwardController;
 
-    private final RelativeEncoder turnEncoder;
-
     private final EncoderConfig turnEncoderConfig;
     private final SparkMaxConfig sparkConfig;
     private final MotorOutputConfigs MotorOutput;
 
-    public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CANcoder absoluteEncoder,
+    public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper turnMotor, CANcoder absoluteEncoder,
                         boolean cancoderInverted, boolean driveInverted, double offset,
                         PIDSettings drivePIDSettings, PIDSettings turnPIDSettings,
                         FeedForwardSettings driveFeedForwardSettings,
@@ -75,7 +64,6 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
         this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings);
-        this.turnEncoder = turnMotor.getEncoder();
         this.sparkConfig = new SparkMaxConfig();
         MotorOutput = new MotorOutputConfigs();
         turnEncoderConfig = new EncoderConfig();
@@ -85,33 +73,20 @@ public SwerveModule(String namespace, TalonFX driveMotor, SparkMax turnMotor, CA
     }
 
     public void configureDriveController() {
-        TalonFXConfiguration config = new TalonFXConfiguration();
         //@TODO check if this is correct
-        config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
-                * INCHES_TO_METERS * Math.PI;
-        config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
-                * INCHES_TO_METERS * Math.PI) / SECONDS_IN_MINUTE;
-        driveMotor.getConfigurator().apply(config);
-        Slot0Configs driveConfigs = new Slot0Configs();
-        driveConfigs.kP = drivePIDSettings.getkP();
-        driveConfigs.kI = drivePIDSettings.getkI();
-        driveConfigs.kD = drivePIDSettings.getkD();
-        driveConfigs.kS = driveFeedForwardSettings.getkS();
-        driveConfigs.kV = driveFeedForwardSettings.getkV();
-        driveConfigs.kA = driveFeedForwardSettings.getkA();
-        driveMotor.getConfigurator().apply(driveConfigs);
-        driveMotor.getConfigurator().apply(MotorOutput.withInverted(driveInverted ?
-                InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive));
+        driveMotor.setEncoderConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
+                * INCHES_TO_METERS * Math.PI);
+        driveMotor.configurePID(drivePIDSettings);
+        driveMotor.configureFF(driveFeedForwardSettings);
+        driveMotor.setInverted(driveInverted);
     }
 
     public void configureTurnController() {
-        turnEncoderConfig.positionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
-        turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
-        ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
-        turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());
-        turnEncoderConfig.inverted(cancoderInverted);
-        sparkConfig.apply(turnClosedLoopConfig);
-        turnMotor.configure(sparkConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+        turnMotor.setPositionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnMotor.setVelocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
+        turnMotor.configurePID(turnPIDSettings);
+        turnMotor.setInverted(cancoderInverted);
+        turnMotor.configureFF(turnFeedForwardSettings);
     }
 
     public void configureAbsoluteEncoder() {
@@ -122,23 +97,16 @@ public void configureAbsoluteEncoder() {
         absoluteEncoder.getConfigurator().apply(magnetConfigs);
     }
 
-    private void turnConfigureFF() {
-        turnFeedForwardController.setGains(turnFeedForwardSettings);
-    }
-
     private void setSpeed(double speed, boolean usePID) {
         if (usePID) {
-            configureDriveController();
-            driveMotor.setControl(new VelocityDutyCycle(speed));
+            driveMotor.pidSet(UnifiedControlMode.VELOCITY, speed, drivePIDSettings, driveFeedForwardSettings,
+                    true);
         } else driveMotor.set(speed / Drivetrain.MAX_SPEED);
     }
 
     private void setAngle(double angle) {
-        configureTurnController();
-        turnConfigureFF();
-        double feedForward = turnFeedForwardController.calculate( 0,angle);
-        turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity, ClosedLoopSlot.kSlot0,
-                feedForward, SparkClosedLoopController.ArbFFUnits.kPercentOut);
+        turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings,
+                true);
     }
 
     public void stop() {
@@ -151,7 +119,7 @@ public void set(SwerveModuleState state, boolean usePID) {
             stop();
             return;
         }
-        state = optimize(state, turnEncoder.getPosition());
+        state = optimize(state, turnMotor.getPosition());
         setAngle(state.angle.getDegrees());
         setSpeed(state.speedMetersPerSecond, usePID);
     }
@@ -182,24 +150,12 @@ public double getAbsoluteAngle() {
     }
 
     public void resetRelativeEncoder() {
-        turnEncoder.setPosition(getAbsoluteAngle());
-    }
-
-    public SwerveModulePosition getPosition() {
-        return new SwerveModulePosition(driveMotor.getPosition().getValueAsDouble(),
-                Rotation2d.fromDegrees(getAbsoluteAngle()));
-    }
-
-    public SwerveModuleState getState() {
-        return new SwerveModuleState(driveMotor.getVelocity().getValueAsDouble(),
-                Rotation2d.fromDegrees(getAbsoluteAngle()));
+        turnMotor.setPosition(getAbsoluteAngle());
     }
 
-
-
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);
-        namespace.putNumber("relative angle", turnEncoder::getPosition);
+        namespace.putNumber("relative angle", turnMotor::getPosition);
     }
 }

From b03cb1c6800dead2a8d8bde73a28e4d5bc1b606e Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:40:04 +0200
Subject: [PATCH 091/158] set default value for pid and ff to 0

---
 .../java/frc/robot/subsystems/SwerveModuleHolder.java     | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index ae328f1..474e72a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -37,13 +37,13 @@ public class SwerveModuleHolder {
     private static final double BACK_RIGHT_OFFSET = -1;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(-1, -1, -1, -1, -1, -1));
+            new PIDSettings(0, 0, 0, 0, 0,0));
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(-1, -1, -1, -1, -1, -1));
+            new PIDSettings(0, 0, 0, 0, 0,0));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "drive", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_VELOCITY));
+            "drive", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "turn", new FeedForwardSettings(-1, -1, -1, -1, FeedForwardController.ControlMode.LINEAR_POSITION));
+            "turn", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_POSITION));
 
     private static SwerveModule frontLeft;
     private static SwerveModule frontRight;

From 63354b4f89ce3b2cdda364b1384bb597e6cd34fa Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 17:41:33 +0200
Subject: [PATCH 092/158] reduced min speed

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 69a467c..56d7a65 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -15,7 +15,7 @@
 public class Drivetrain extends DashboardedSubsystem {
 
     public static final double MAX_SPEED = 4;
-    public static final double MIN_SPEED = 0.4;
+    public static final double MIN_SPEED = 0.05;
     public static final double MAX_TURN_SPEED = 3;
 
     private static final double TRACK_WIDTH = -1;

From 45fd5379cc4e24e48594d622fdec8392c4cd023c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:52:04 +0200
Subject: [PATCH 093/158] deleted todo

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 56d7a65..7b49e38 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -48,7 +48,6 @@ public static Drivetrain getInstance() {
         if (instance == null) {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
-                    //TODO check in shuffleboard if this is correct
                     new AHRS(AHRS.NavXComType.kMXP_SPI));
         }
         return instance;

From 6a04e1fab63d135bff6212e0f931111f0be52cfa Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:52:31 +0200
Subject: [PATCH 094/158] deleted extra line

---
 .../java/frc/robot/subsystems/Drivetrain.java | 79 ++-----------------
 1 file changed, 5 insertions(+), 74 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 7b49e38..9956270 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,16 +1,12 @@
 package frc.robot.subsystems;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.config.RobotConfig;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.spikes2212.command.DashboardedSubsystem;
 import com.studica.frc.AHRS;
-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.kinematics.*;
-import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SerialPort;
 
 public class Drivetrain extends DashboardedSubsystem {
 
@@ -40,8 +36,6 @@ public class Drivetrain extends DashboardedSubsystem {
     private final AHRS gyro;
     private final SwerveDriveKinematics kinematics;
 
-    private SwerveModulePosition[] stupidPointlessArray;
-    private final SwerveDriveOdometry odometry;
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {
@@ -60,54 +54,9 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
-        stupidPointlessArray = new SwerveModulePosition[]{frontLeft.getPosition(),
-                frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
+        this.gyro = gyro;
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
-        this.gyro = gyro;
-        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), stupidPointlessArray,
-                new Pose2d());
-        RobotConfig config;
-        try{
-            config = RobotConfig.fromGUISettings();
-        } catch (Exception e) {
-            // Handle exception as needed
-            e.printStackTrace();
-            config = null;
-        }
-
-        // Configure AutoBuilder last
-        AutoBuilder.configure(
-                this::getPose2d, // Robot pose supplier
-                this::resetPose2d, // Method to reset odometry (will be called if your auto has a starting pose)
-                this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
-                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
-                        new PIDConstants(0, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(0.01, 0.0, 0.0) // Rotation PID constants
-                ),
-                config, // The robot configuration
-                () -> {
-                    // Boolean supplier that controls when the path will be mirrored for the red alliance
-                    // This will flip the path being followed to the red side of the field.
-                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-
-                    var alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
-                    }
-                    return false;
-                },
-                this // Reference to this subsystem to set requirements
-        );
-    }
-
-    @Override
-    public void periodic() {
-        super.periodic();
-        stupidPointlessArray = new SwerveModulePosition[]{frontLeft.getPosition(),
-                frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
-        odometry.update(gyro.getRotation2d(), stupidPointlessArray);
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
@@ -142,24 +91,6 @@ public void resetRelativeEncoder() {
         backRight.resetRelativeEncoder();
     }
 
-    public Pose2d getPose2d() {
-        return odometry.getPoseMeters();
-    }
-
-    public void resetPose2d(Pose2d desiredPose) {
-        odometry.resetPosition(gyro.getRotation2d(), stupidPointlessArray, desiredPose);
-    }
-
-    public ChassisSpeeds getRobotRelativeSpeeds() {
-        return kinematics.toChassisSpeeds(frontLeft.getState(), frontRight.getState(),
-                backLeft.getState(), backRight.getState());
-    }
-
-    public void driveRobotRelative(ChassisSpeeds speeds) {
-        drive(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond,
-                false, true);
-    }
-
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro", gyro::getAngle);

From dd40cd48c4981b09a699e41482c0e8351eddc7bf Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:53:39 +0200
Subject: [PATCH 095/158] specified what about the gyro

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9956270..3b1294f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -93,6 +93,6 @@ public void resetRelativeEncoder() {
 
     @Override
     public void configureDashboard() {
-        namespace.putNumber("gyro", gyro::getAngle);
+        namespace.putNumber("gyro yaw", gyro::getAngle);
     }
 }

From 197d95d84f6686562fdfbb2a5873f08861fd6df9 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 18:55:23 +0200
Subject: [PATCH 096/158] removed unnecessary fields

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 8aa059c..5edb131 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -41,11 +41,6 @@ public class SwerveModule extends DashboardedSubsystem {
     private final PIDSettings turnPIDSettings;
     private final FeedForwardSettings driveFeedForwardSettings;
     private final FeedForwardSettings turnFeedForwardSettings;
-    private final FeedForwardController turnFeedForwardController;
-
-    private final EncoderConfig turnEncoderConfig;
-    private final SparkMaxConfig sparkConfig;
-    private final MotorOutputConfigs MotorOutput;
 
     public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper turnMotor, CANcoder absoluteEncoder,
                         boolean cancoderInverted, boolean driveInverted, double offset,
@@ -63,10 +58,6 @@ public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper tu
         this.turnPIDSettings = turnPIDSettings;
         this.driveFeedForwardSettings = driveFeedForwardSettings;
         this.turnFeedForwardSettings = turnFeedForwardSettings;
-        this.turnFeedForwardController = new FeedForwardController(turnFeedForwardSettings);
-        this.sparkConfig = new SparkMaxConfig();
-        MotorOutput = new MotorOutputConfigs();
-        turnEncoderConfig = new EncoderConfig();
         configureDriveController();
         configureTurnController();
         configureAbsoluteEncoder();

From c8a493303ed11eeb933647b1ac5591fc7c5656ce Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 19:13:03 +0200
Subject: [PATCH 097/158] removed unnecessary pid configurations

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 5edb131..8195845 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -67,17 +67,13 @@ public void configureDriveController() {
         //@TODO check if this is correct
         driveMotor.setEncoderConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
                 * INCHES_TO_METERS * Math.PI);
-        driveMotor.configurePID(drivePIDSettings);
-        driveMotor.configureFF(driveFeedForwardSettings);
         driveMotor.setInverted(driveInverted);
     }
 
     public void configureTurnController() {
         turnMotor.setPositionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
         turnMotor.setVelocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
-        turnMotor.configurePID(turnPIDSettings);
         turnMotor.setInverted(cancoderInverted);
-        turnMotor.configureFF(turnFeedForwardSettings);
     }
 
     public void configureAbsoluteEncoder() {

From 5186e91d0491eb2e8528cd127ef411eb5417c8fd Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 19:14:34 +0200
Subject: [PATCH 098/158] added blank line at the end of class

---
 vendordeps/SpikesLib2.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
index f04853c..bc10370 100644
--- a/vendordeps/SpikesLib2.json
+++ b/vendordeps/SpikesLib2.json
@@ -18,4 +18,4 @@
       "version": "v5.1.0"
     }
   ]
-}
\ No newline at end of file
+}

From b9b19793ce91c2cda410e7edc7c17421ca0ded2b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 19:58:34 +0200
Subject: [PATCH 099/158] renamed reset relative encoders func

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 3b1294f..38cf0d0 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -84,7 +84,7 @@ public void stop() {
         backRight.stop();
     }
 
-    public void resetRelativeEncoder() {
+    public void resetRelativeEncoders() {
         frontLeft.resetRelativeEncoder();
         frontRight.resetRelativeEncoder();
         backLeft.resetRelativeEncoder();

From 4c8eed6860eed539be9398456a0c9b394fe254e6 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 20:00:06 +0200
Subject: [PATCH 100/158] fixed unnecessary this call

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 7f30c62..f359e0c 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -32,7 +32,7 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
         this.usePID = usePID;
         this.xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         this.yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
-        this.rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
+        rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
     }
 
     @Override

From e5da446ea111298e20d9027f361c61fa4af16f30 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 20:48:53 +0200
Subject: [PATCH 101/158] added track variables in cm

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 38cf0d0..3cc2747 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -14,8 +14,8 @@ public class Drivetrain extends DashboardedSubsystem {
     public static final double MIN_SPEED = 0.05;
     public static final double MAX_TURN_SPEED = 3;
 
-    private static final double TRACK_WIDTH = -1;
-    private static final double TRACK_LENGTH = -1;
+    private static final double TRACK_WIDTH = 60;
+    private static final double TRACK_LENGTH = 60;
 
     private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     private static final Translation2d FRONT_LEFT_WHEEL_POSITION =

From 1b0c287058aabad81f732f58eb9ac35a1438f79b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 21:32:47 +0200
Subject: [PATCH 102/158] removed unnecessary this call

---
 src/main/java/frc/robot/commands/Drive.java | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index f359e0c..c853b57 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -25,13 +25,14 @@ public class Drive extends Command {
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
         this.drivetrain = drivetrain;
+        addRequirements(drivetrain);
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationsSpeed = rotationsSpeed;
         this.fieldRelative = fieldRelative;
         this.usePID = usePID;
-        this.xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
-        this.yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
     }
 

From f5f644b370717e1b564d89ab2a38d46c0e995845 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:31:35 +0200
Subject: [PATCH 103/158] renamed to rotationSpeed

---
 src/main/java/frc/robot/commands/Drive.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index c853b57..9a03382 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -14,7 +14,7 @@ public class Drive extends Command {
     private final Drivetrain drivetrain;
     private final Supplier<Double> xSpeed;
     private final Supplier<Double> ySpeed;
-    private final Supplier<Double> rotationsSpeed;
+    private final Supplier<Double> rotationSpeed;
     private final boolean fieldRelative;
     private final boolean usePID;
 
@@ -23,12 +23,12 @@ public class Drive extends Command {
     private final SlewRateLimiter rotationLimiter;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
-                 Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
+                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID) {
         this.drivetrain = drivetrain;
         addRequirements(drivetrain);
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
-        this.rotationsSpeed = rotationsSpeed;
+        this.rotationSpeed = rotationSpeed;
         this.fieldRelative = fieldRelative;
         this.usePID = usePID;
         xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
@@ -40,7 +40,7 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
     public void execute() {
         double xSpeed = xLimiter.calculate(this.xSpeed.get());
         double ySpeed = yLimiter.calculate(this.ySpeed.get());
-        double rotationSpeed = rotationLimiter.calculate(this.rotationsSpeed.get());
+        double rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 

From 68c45dce32bdbc664a228e5913b4b550e8c87175 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:41:07 +0200
Subject: [PATCH 104/158] merged into 1 line

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 8195845..cba4d73 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -102,7 +102,7 @@ public void stop() {
     }
 
     public void set(SwerveModuleState state, boolean usePID) {
-        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED){
+        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED) {
             stop();
             return;
         }
@@ -123,10 +123,7 @@ private SwerveModuleState optimize(SwerveModuleState state, double currentAngle)
         while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
-            }
-            else {
-                desiredAngle += DEGREES_TO_FLIP;
-            }
+            } else {desiredAngle += DEGREES_TO_FLIP;}
             state.speedMetersPerSecond *= -1;
         }
         return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));

From 25c7d1e46b6214f10386c4dad7cff10efb357bcb Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:43:45 +0200
Subject: [PATCH 105/158] accommodated minus

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index cba4d73..9bb4840 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -1,13 +1,9 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-import com.revrobotics.spark.config.EncoderConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
 import com.spikes2212.command.DashboardedSubsystem;
-import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
 import com.spikes2212.util.UnifiedControlMode;
@@ -16,6 +12,8 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
+import static java.lang.Math.abs;
+
 public class SwerveModule extends DashboardedSubsystem {
 
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
@@ -102,7 +100,7 @@ public void stop() {
     }
 
     public void set(SwerveModuleState state, boolean usePID) {
-        if (state.speedMetersPerSecond < Drivetrain.MIN_SPEED) {
+        if (Math.abs(state.speedMetersPerSecond) < Drivetrain.MIN_SPEED) {
             stop();
             return;
         }
@@ -120,7 +118,7 @@ private double normalizeAngleRelativeToEncoder(double currentAngle, double desir
 
     private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
         double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
-        while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
+        while (abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
             } else {desiredAngle += DEGREES_TO_FLIP;}

From 9013a50219ffaed4e964a3a9e0945af553361397 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:44:28 +0200
Subject: [PATCH 106/158] 0 default offset value

---
 .../java/frc/robot/subsystems/SwerveModuleHolder.java     | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 474e72a..0d07061 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -31,10 +31,10 @@ public class SwerveModuleHolder {
     private static final String BACK_LEFT_NAMESPACE_NAME = "back left";
     private static final String BACK_RIGHT_NAMESPACE_NAME = "back right";
 
-    private static final double FRONT_LEFT_OFFSET = -1;
-    private static final double FRONT_RIGHT_OFFSET = -1;
-    private static final double BACK_LEFT_OFFSET = -1;
-    private static final double BACK_RIGHT_OFFSET = -1;
+    private static final double FRONT_LEFT_OFFSET = 0;
+    private static final double FRONT_RIGHT_OFFSET = 0;
+    private static final double BACK_LEFT_OFFSET = 0;
+    private static final double BACK_RIGHT_OFFSET = 0;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
             new PIDSettings(0, 0, 0, 0, 0,0));

From 58f96c58b28e4cccb46e479b3c521cb949c35454 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:47:02 +0200
Subject: [PATCH 107/158] removed unnecessary values

---
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 0d07061..b14e179 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -37,9 +37,9 @@ public class SwerveModuleHolder {
     private static final double BACK_RIGHT_OFFSET = 0;
 
     private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(0, 0, 0, 0, 0,0));
+            new PIDSettings(0, 0, 0));
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(0, 0, 0, 0, 0,0));
+            new PIDSettings(0, 0, 0));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
             "drive", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(

From dfa9f73525b215bffc99fae19a02f5adca127b4a Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 19:47:55 +0200
Subject: [PATCH 108/158] ditto

---
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index b14e179..ab05e88 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -41,9 +41,9 @@ public class SwerveModuleHolder {
     private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
             new PIDSettings(0, 0, 0));
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "drive", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_VELOCITY));
+            "drive", new FeedForwardSettings(FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(
-            "turn", new FeedForwardSettings(0, 0, 0, 0, FeedForwardController.ControlMode.LINEAR_POSITION));
+            "turn", new FeedForwardSettings(FeedForwardController.ControlMode.LINEAR_POSITION));
 
     private static SwerveModule frontLeft;
     private static SwerveModule frontRight;

From 6d501887b5dcea068f54c66aadc8958f6b648813 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:10:07 +0200
Subject: [PATCH 109/158] unmerged

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 9bb4840..55809a1 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -121,7 +121,9 @@ private SwerveModuleState optimize(SwerveModuleState state, double currentAngle)
         while (abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
-            } else {desiredAngle += DEGREES_TO_FLIP;}
+            } else {
+                desiredAngle += DEGREES_TO_FLIP;
+            }
             state.speedMetersPerSecond *= -1;
         }
         return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));

From 50909619e35553a57d3a7d24f1231ffc5f6d9bd2 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:11:14 +0200
Subject: [PATCH 110/158] deleted unnecessary imports

---
 src/main/java/frc/robot/subsystems/Drivetrain.java         | 1 -
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 2 --
 2 files changed, 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 3cc2747..a1af8ea 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.SerialPort;
 
 public class Drivetrain extends DashboardedSubsystem {
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index ab05e88..03c9632 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -1,9 +1,7 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.spark.SparkLowLevel;
-import com.revrobotics.spark.SparkMax;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;

From b6b5796207dfe84d0cf9eaf8fb85840eb8fee10b Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:12:10 +0200
Subject: [PATCH 111/158] deleted weird file

---
 src/main/deploy/pathplanner/navgrid.json | 1 -
 1 file changed, 1 deletion(-)
 delete mode 100644 src/main/deploy/pathplanner/navgrid.json

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
deleted file mode 100644
index 23e0db9..0000000
--- a/src/main/deploy/pathplanner/navgrid.json
+++ /dev/null
@@ -1 +0,0 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From 35082cae65980522ef193254e2e9ad0734698a03 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:14:38 +0200
Subject: [PATCH 112/158] deleted unnecessary given values

---
 src/main/java/frc/robot/subsystems/SwerveModuleHolder.java | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
index 03c9632..1266886 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
@@ -34,10 +34,8 @@ public class SwerveModuleHolder {
     private static final double BACK_LEFT_OFFSET = 0;
     private static final double BACK_RIGHT_OFFSET = 0;
 
-    private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive",
-            new PIDSettings(0, 0, 0));
-    private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn",
-            new PIDSettings(0, 0, 0));
+    private static final PIDSettings drivePIDSettings = namespace.addPIDNamespace("drive");
+    private static final PIDSettings turnPIDSettings = namespace.addPIDNamespace("turn");
     private static final FeedForwardSettings driveFeedForwardSettings = namespace.addFeedForwardNamespace(
             "drive", new FeedForwardSettings(FeedForwardController.ControlMode.LINEAR_VELOCITY));
     private static final FeedForwardSettings turnFeedForwardSettings = namespace.addFeedForwardNamespace(

From 60ebc9c97c7632e5e1836901865e8f9c755e3651 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:37:19 +0200
Subject: [PATCH 113/158] add requirements FIRST (get it??? like FIRST the
 program, do you get it??)

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 9a03382..e8565a9 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -24,8 +24,8 @@ public class Drive extends Command {
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID) {
-        this.drivetrain = drivetrain;
         addRequirements(drivetrain);
+        this.drivetrain = drivetrain;
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationSpeed = rotationSpeed;

From 167bc74f785e5c56c5d067a7890cc447ea55e339 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:40:06 +0200
Subject: [PATCH 114/158] changed units to meters

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index a1af8ea..40cc0ae 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,8 +13,8 @@ public class Drivetrain extends DashboardedSubsystem {
     public static final double MIN_SPEED = 0.05;
     public static final double MAX_TURN_SPEED = 3;
 
-    private static final double TRACK_WIDTH = 60;
-    private static final double TRACK_LENGTH = 60;
+    private static final double TRACK_WIDTH = 0.6;
+    private static final double TRACK_LENGTH = 0.6;
 
     private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     private static final Translation2d FRONT_LEFT_WHEEL_POSITION =

From f0739dcc56d0df2be45680fa2702ef91b2d1a6cb Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:41:20 +0200
Subject: [PATCH 115/158] called configureDashboard

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 40cc0ae..2e67e88 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -56,6 +56,7 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.gyro = gyro;
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        configureDashboard();
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,

From bd130aa6c3dfef5ec6f13856763b8fdbc02d513c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:42:32 +0200
Subject: [PATCH 116/158] ditto

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 55809a1..95796f8 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -59,6 +59,7 @@ public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper tu
         configureDriveController();
         configureTurnController();
         configureAbsoluteEncoder();
+        configureDashboard();
     }
 
     public void configureDriveController() {

From 7b22dd936219389c24bd160cfeddf16fce0fc710 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 30 Jan 2025 20:43:50 +0200
Subject: [PATCH 117/158] boom.

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 95796f8..51e1517 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -12,8 +12,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
-import static java.lang.Math.abs;
-
 public class SwerveModule extends DashboardedSubsystem {
 
     private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
@@ -119,7 +117,7 @@ private double normalizeAngleRelativeToEncoder(double currentAngle, double desir
 
     private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
         double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
-        while (abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
+        while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
             if (desiredAngle - currentAngle > 0) {
                 desiredAngle -= DEGREES_TO_FLIP;
             } else {

From ca04dd2a587ad285ce7d186bddab04c9d332cab6 Mon Sep 17 00:00:00 2001
From: TurtleMinecraft <schnitzelgirl69@gmail.com>
Date: Fri, 31 Jan 2025 18:09:16 +0200
Subject: [PATCH 118/158] updated libraries

---
 .idea/jarRepositories.xml                     |  5 ++
 ....2.1.json => PathplannerLib-2025.2.2.json} |  8 +-
 ...enix6-25.1.0.json => Phoenix6-25.2.1.json} | 84 +++++++++++++------
 ...VLib-2025.0.0.json => REVLib-2025.0.2json} | 15 ++--
 ...ca-2025.0.0.json => Studica-2025.0.1.json} | 14 ++--
 vendordeps/photonlib.json                     | 12 +--
 6 files changed, 85 insertions(+), 53 deletions(-)
 rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.2.json} (87%)
 rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.1.json} (85%)
 rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.2json} (86%)
 rename vendordeps/{Studica-2025.0.0.json => Studica-2025.0.1.json} (88%)

diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index 02e25a8..a105306 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -111,5 +111,10 @@
       <option name="name" value="WPIcb311d09-36e9-4143-a032-55bb2b94443b_0Release" />
       <option name="url" value="https://dev.studica.com/maven/release/2025/" />
     </remote-repository>
+    <remote-repository>
+      <option name="id" value="WPILocal" />
+      <option name="name" value="WPILocal" />
+      <option name="url" value="file:/$PROJECT_DIR$/../../../../../../Public/wpilib/2025/maven" />
+    </remote-repository>
   </component>
 </project>
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.2.1.json
rename to vendordeps/PathplannerLib-2025.2.2.json
index 731bf22..c0f7c11 100644
--- a/vendordeps/PathplannerLib-2025.2.1.json
+++ b/vendordeps/PathplannerLib-2025.2.2.json
@@ -1,7 +1,7 @@
 {
-  "fileName": "PathplannerLib-2025.2.1.json",
+  "fileName": "PathplannerLib.json",
   "name": "PathplannerLib",
-  "version": "2025.2.1",
+  "version": "2025.2.2",
   "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
   "frcYear": "2025",
   "mavenUrls": [
@@ -12,7 +12,7 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-java",
-      "version": "2025.2.1"
+      "version": "2025.2.2"
     }
   ],
   "jniDependencies": [],
@@ -20,7 +20,7 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-cpp",
-      "version": "2025.2.1",
+      "version": "2025.2.2",
       "libName": "PathplannerLib",
       "headerClassifier": "headers",
       "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.1.json
similarity index 85%
rename from vendordeps/Phoenix6-25.1.0.json
rename to vendordeps/Phoenix6-25.2.1.json
index 18462c6..349f124 100644
--- a/vendordeps/Phoenix6-25.1.0.json
+++ b/vendordeps/Phoenix6-25.2.1.json
@@ -1,7 +1,7 @@
 {
-  "fileName": "Phoenix6-25.1.0.json",
+  "fileName": "Phoenix6-25.2.1.json",
   "name": "CTRE-Phoenix (v6)",
-  "version": "25.1.0",
+  "version": "25.2.1",
   "frcYear": "2025",
   "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
   "mavenUrls": [
@@ -19,14 +19,14 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "wpiapi-java",
-      "version": "25.1.0"
+      "version": "25.2.1"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "api-cpp",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -40,7 +40,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "tools",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -54,7 +54,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "api-cpp-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -68,7 +68,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "tools-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -82,7 +82,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simTalonSRX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -96,7 +96,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simVictorSPX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -110,7 +110,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simPigeonIMU",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -124,7 +124,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simCANCoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -138,7 +138,21 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFX",
-      "version": "25.1.0",
+      "version": "25.2.1",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProTalonFXS",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -152,7 +166,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANcoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -166,7 +180,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProPigeon2",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -180,7 +194,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANrange",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -196,7 +210,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "wpiapi-cpp",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_Phoenix6_WPI",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -212,7 +226,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "tools",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_PhoenixTools",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -228,7 +242,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "wpiapi-cpp-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_Phoenix6_WPISim",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -244,7 +258,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "tools-sim",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_PhoenixTools_Sim",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -260,7 +274,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simTalonSRX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimTalonSRX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -276,7 +290,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simVictorSPX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimVictorSPX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -292,7 +306,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simPigeonIMU",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimPigeonIMU",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -308,7 +322,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simCANCoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimCANCoder",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -324,7 +338,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFX",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProTalonFX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -337,10 +351,26 @@
       ],
       "simMode": "swsim"
     },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProTalonFXS",
+      "version": "25.2.1",
+      "libName": "CTRE_SimProTalonFXS",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANcoder",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProCANcoder",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -356,7 +386,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProPigeon2",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProPigeon2",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -372,7 +402,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANrange",
-      "version": "25.1.0",
+      "version": "25.2.1",
       "libName": "CTRE_SimProCANrange",
       "headerClassifier": "headers",
       "sharedLibrary": true,
diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.2json
similarity index 86%
rename from vendordeps/REVLib-2025.0.0.json
rename to vendordeps/REVLib-2025.0.2json
index 1a4f3cd..30ea8de 100644
--- a/vendordeps/REVLib-2025.0.0.json
+++ b/vendordeps/REVLib-2025.0.2json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.0.json",
+    "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json
similarity index 88%
rename from vendordeps/Studica-2025.0.0.json
rename to vendordeps/Studica-2025.0.1.json
index 8cba530..a3ed1fe 100644
--- a/vendordeps/Studica-2025.0.0.json
+++ b/vendordeps/Studica-2025.0.1.json
@@ -1,13 +1,13 @@
 {
-  "fileName": "Studica-2025.0.0.json",
+  "fileName": "Studica-2025.0.1.json",
   "name": "Studica",
-  "version": "2025.0.0",
+  "version": "2025.0.1",
   "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
   "frcYear": "2025",
   "mavenUrls": [
     "https://dev.studica.com/maven/release/2025/"
   ],
-  "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json",
+  "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
   "cppDependencies": [
     {
       "artifactId": "Studica-cpp",
@@ -24,7 +24,7 @@
       "libName": "Studica",
       "sharedLibrary": false,
       "skipInvalidPlatforms": true,
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     },
     {
       "artifactId": "Studica-driver",
@@ -41,14 +41,14 @@
       "libName": "StudicaDriver",
       "sharedLibrary": false,
       "skipInvalidPlatforms": true,
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     }
   ],
   "javaDependencies": [
     {
       "artifactId": "Studica-java",
       "groupId": "com.studica.frc",
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     }
   ],
   "jniDependencies": [
@@ -65,7 +65,7 @@
         "osxuniversal",
         "windowsx86-64"
       ],
-      "version": "2025.0.0"
+      "version": "2025.0.1"
     }
   ]
 }
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 1163429..a908923 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2025.0.0-beta-8",
+  "version": "v2025.1.1",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "frcYear": "2025",
   "mavenUrls": [
@@ -13,7 +13,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2025.0.0-beta-8",
+      "version": "v2025.1.1",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -28,7 +28,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2025.0.0-beta-8",
+      "version": "v2025.1.1",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -43,7 +43,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2025.0.0-beta-8",
+      "version": "v2025.1.1",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -60,12 +60,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2025.0.0-beta-8"
+      "version": "v2025.1.1"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2025.0.0-beta-8"
+      "version": "v2025.1.1"
     }
   ]
 }

From f3b60a927df23a54c7bb31979828d2cccd83ea3c Mon Sep 17 00:00:00 2001
From: TurtleMinecraft <schnitzelgirl69@gmail.com>
Date: Fri, 31 Jan 2025 18:11:06 +0200
Subject: [PATCH 119/158] bruh

---
 vendordeps/{REVLib-2025.0.2json => REVLib-2025.0.2.json} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename vendordeps/{REVLib-2025.0.2json => REVLib-2025.0.2.json} (100%)

diff --git a/vendordeps/REVLib-2025.0.2json b/vendordeps/REVLib-2025.0.2.json
similarity index 100%
rename from vendordeps/REVLib-2025.0.2json
rename to vendordeps/REVLib-2025.0.2.json

From d7607a0b34d2e40e23466eefd361b6527b61c15d Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 18:37:39 +0200
Subject: [PATCH 120/158] not done yet

---
 src/main/java/frc/robot/RobotMap.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 3f149cc..2b2e462 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -4,17 +4,17 @@ public class RobotMap {
 
     public interface CAN {
 
-        int FRONT_LEFT_DRIVE_TALON_FX = -1;
-        int FRONT_RIGHT_DRIVE_TALON_FX = -1;
+        int FRONT_LEFT_DRIVE_TALON_FX = 1;
+        int FRONT_RIGHT_DRIVE_TALON_FX = 4;
         int BACK_LEFT_DRIVE_TALON_FX = -1;
         int BACK_RIGHT_DRIVE_TALON_FX = -1;
 
-        int FRONT_LEFT_TURN_SPARK_MAX = -1;
+        int FRONT_LEFT_TURN_SPARK_MAX = 2;
         int FRONT_RIGHT_TURN_SPARK_MAX = -1;
         int BACK_LEFT_TURN_SPARK_MAX = -1;
         int BACK_RIGHT_TURN_SPARK_MAX = -1;
 
-        int FRONT_LEFT_ABSOLUTE_ENCODER = -1;
+        int FRONT_LEFT_ABSOLUTE_ENCODER = 3;
         int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
         int BACK_LEFT_ABSOLUTE_ENCODER = -1;
         int BACK_RIGHT_ABSOLUTE_ENCODER = -1;

From 010249cf4afc4bfb844e7b4102280c46d89079c0 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 18:46:13 +0200
Subject: [PATCH 121/158] updated CAN ports

---
 src/main/java/frc/robot/RobotMap.java | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 2b2e462..9b94f82 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -6,18 +6,18 @@ public interface CAN {
 
         int FRONT_LEFT_DRIVE_TALON_FX = 1;
         int FRONT_RIGHT_DRIVE_TALON_FX = 4;
-        int BACK_LEFT_DRIVE_TALON_FX = -1;
-        int BACK_RIGHT_DRIVE_TALON_FX = -1;
+        int BACK_LEFT_DRIVE_TALON_FX = 7;
+        int BACK_RIGHT_DRIVE_TALON_FX = 10;
 
         int FRONT_LEFT_TURN_SPARK_MAX = 2;
-        int FRONT_RIGHT_TURN_SPARK_MAX = -1;
-        int BACK_LEFT_TURN_SPARK_MAX = -1;
-        int BACK_RIGHT_TURN_SPARK_MAX = -1;
+        int FRONT_RIGHT_TURN_SPARK_MAX = 5;
+        int BACK_LEFT_TURN_SPARK_MAX = 8;
+        int BACK_RIGHT_TURN_SPARK_MAX = 11;
 
         int FRONT_LEFT_ABSOLUTE_ENCODER = 3;
-        int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
-        int BACK_LEFT_ABSOLUTE_ENCODER = -1;
-        int BACK_RIGHT_ABSOLUTE_ENCODER = -1;
+        int FRONT_RIGHT_ABSOLUTE_ENCODER = 6;
+        int BACK_LEFT_ABSOLUTE_ENCODER = 9;
+        int BACK_RIGHT_ABSOLUTE_ENCODER = 12;
     }
     
     public interface DIO {

From 44fdd95e6a6d80eaf59997b157230f947ee361f0 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 18:48:59 +0200
Subject: [PATCH 122/158] added reset gyro func

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 2e67e88..9b02a28 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -91,6 +91,10 @@ public void resetRelativeEncoders() {
         backRight.resetRelativeEncoder();
     }
 
+    public void resetGyro() {
+        gyro.reset();
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);

From 24a18dc873a76ad4084ebfc01610aa905b916b60 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 19:03:04 +0200
Subject: [PATCH 123/158] added option to disable acceleration

---
 src/main/java/frc/robot/commands/Drive.java | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index e8565a9..476aec5 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -21,16 +21,19 @@ public class Drive extends Command {
     private final SlewRateLimiter xLimiter;
     private final SlewRateLimiter yLimiter;
     private final SlewRateLimiter rotationLimiter;
+    private final boolean limitAcceleration;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
-                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID) {
+                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,
+                 boolean limitAcceleration) {
         addRequirements(drivetrain);
         this.drivetrain = drivetrain;
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationSpeed = rotationSpeed;
-        this.fieldRelative = fieldRelative;
         this.usePID = usePID;
+        this.limitAcceleration = limitAcceleration;
+        this.fieldRelative = fieldRelative;
         xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
@@ -38,9 +41,16 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
 
     @Override
     public void execute() {
-        double xSpeed = xLimiter.calculate(this.xSpeed.get());
-        double ySpeed = yLimiter.calculate(this.ySpeed.get());
-        double rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
+        double xSpeed = this.xSpeed.get();
+        double ySpeed = this.ySpeed.get();
+        double rotationSpeed = this.rotationSpeed.get();
+
+        if (limitAcceleration) {
+            xSpeed = xLimiter.calculate(this.xSpeed.get());
+            ySpeed = yLimiter.calculate(this.ySpeed.get());
+            rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
+        }
+
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 

From eccaa12bdee0096291170e35ce03e6c37c7613ac Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Fri, 31 Jan 2025 19:04:59 +0200
Subject: [PATCH 124/158] updated SpikesLib2.jsonl

---
 vendordeps/SpikesLib2.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
index bc10370..1719631 100644
--- a/vendordeps/SpikesLib2.json
+++ b/vendordeps/SpikesLib2.json
@@ -15,7 +15,7 @@
     {
       "groupId": "com.github.Spikes-2212-Programming-Guild",
       "artifactId": "SpikesLib2",
-      "version": "v5.1.0"
+      "version": "v5.1.1"
     }
   ]
 }

From aad018f1936b264628f3b17943f3d94146207a05 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:28:47 +0200
Subject: [PATCH 125/158] updated SpikesLib2.json

---
 vendordeps/SpikesLib2.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendordeps/SpikesLib2.json b/vendordeps/SpikesLib2.json
index 1719631..7397e86 100644
--- a/vendordeps/SpikesLib2.json
+++ b/vendordeps/SpikesLib2.json
@@ -1,7 +1,7 @@
 {
   "fileName": "SpikesLib2.json",
   "name": "SpikesLib2",
-  "version": "v5.1.0",
+  "version": "v5.1.1",
   "frcYear": "2025",
   "uuid": "018a8f54-8662-11ec-a8a3-0242ac120002",
   "jsonUrl": "https://raw.githubusercontent.com/Spikes-2212-Programming-Guild/SpikesLib2/master/SpikesLib2.json",

From e4243eeab8d3dcd9b6fec753a9459c94830abe08 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:46:05 +0200
Subject: [PATCH 126/158] added override to drive command end func

---
 src/main/java/frc/robot/commands/Drive.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 476aec5..c5c534b 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -54,6 +54,7 @@ public void execute() {
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
     }
 
+    @Override
     public void end(boolean interrupted) {
         drivetrain.stop();
     }

From 6f04a7e05be9aaaf1177b8e8990bec8f06cc8c25 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:48:06 +0200
Subject: [PATCH 127/158] limitAcceleration below usePID

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index c5c534b..630be5d 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -17,11 +17,11 @@ public class Drive extends Command {
     private final Supplier<Double> rotationSpeed;
     private final boolean fieldRelative;
     private final boolean usePID;
+    private final boolean limitAcceleration;
 
     private final SlewRateLimiter xLimiter;
     private final SlewRateLimiter yLimiter;
     private final SlewRateLimiter rotationLimiter;
-    private final boolean limitAcceleration;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,

From 4efc0d9f14a3d816744489635a9159ab7919cc74 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:49:44 +0200
Subject: [PATCH 128/158] initializing the values in the same order as received

---
 src/main/java/frc/robot/commands/Drive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 630be5d..f394ebf 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -31,9 +31,9 @@ public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> yS
         this.xSpeed = xSpeed;
         this.ySpeed = ySpeed;
         this.rotationSpeed = rotationSpeed;
+        this.fieldRelative = fieldRelative;
         this.usePID = usePID;
         this.limitAcceleration = limitAcceleration;
-        this.fieldRelative = fieldRelative;
         xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
         rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);

From 3bee86577e2d8249458e28f5c76281d310b516b9 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 09:50:25 +0200
Subject: [PATCH 129/158] added blank line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9b02a28..f010694 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -33,6 +33,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveModule backLeft;
     private final SwerveModule backRight;
     private final AHRS gyro;
+
     private final SwerveDriveKinematics kinematics;
 
     private static Drivetrain instance;

From 2fb6e951b0415322f3153ef45a3e1593039237d1 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 10:30:26 +0200
Subject: [PATCH 130/158] added odometry

---
 .../java/frc/robot/subsystems/Drivetrain.java | 22 +++++++++++++++----
 .../frc/robot/subsystems/SwerveModule.java    |  6 +++++
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index f010694..b185b2f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,10 +2,9 @@
 
 import com.spikes2212.command.DashboardedSubsystem;
 import com.studica.frc.AHRS;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.kinematics.*;
 
 public class Drivetrain extends DashboardedSubsystem {
 
@@ -34,8 +33,11 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveModule backRight;
     private final AHRS gyro;
 
+    private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
+    private static Pose2d pose2d;
+
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {
@@ -48,15 +50,19 @@ public static Drivetrain getInstance() {
     }
 
     private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
-                      SwerveModule backRight, AHRS gyro) {
+                       SwerveModule backRight, AHRS gyro) {
         super(NAMESPACE_NAME);
         this.frontLeft = frontLeft;
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
         this.gyro = gyro;
+        this.pose2d = new Pose2d();
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
+                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
+                 backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()});
         configureDashboard();
     }
 
@@ -96,6 +102,14 @@ public void resetGyro() {
         gyro.reset();
     }
 
+    @Override
+    public void periodic() {
+        pose2d = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
+                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
+                backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()
+        });
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 51e1517..10ba995 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -9,7 +9,9 @@
 import com.spikes2212.util.UnifiedControlMode;
 import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
 import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
 public class SwerveModule extends DashboardedSubsystem {
@@ -136,6 +138,10 @@ public void resetRelativeEncoder() {
         turnMotor.setPosition(getAbsoluteAngle());
     }
 
+    public SwerveModulePosition getSwerveModulePosition() {
+        return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(turnMotor.getPosition()));
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);

From b7bba406d66bc3e602f6fba811d56bc87a005c31 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:15:25 +0200
Subject: [PATCH 131/158] renamed currentPose and removed this

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index b185b2f..fb43d7f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -36,7 +36,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
-    private static Pose2d pose2d;
+    private static Pose2d currentPose;
 
     private static Drivetrain instance;
 
@@ -57,7 +57,7 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.backLeft = backLeft;
         this.backRight = backRight;
         this.gyro = gyro;
-        this.pose2d = new Pose2d();
+        currentPose = new Pose2d();
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
@@ -104,7 +104,7 @@ public void resetGyro() {
 
     @Override
     public void periodic() {
-        pose2d = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
+        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
                 frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
                 backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()
         });

From 2e1e3d9635dc279194351e739bf260e4631e7d51 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:16:43 +0200
Subject: [PATCH 132/158] renamed getModulePosition

---
 src/main/java/frc/robot/subsystems/Drivetrain.java   | 8 ++++----
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index fb43d7f..9c94d8b 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -61,8 +61,8 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
-                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
-                 backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()});
+                frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                 backLeft.getModulePosition(), backRight.getModulePosition()});
         configureDashboard();
     }
 
@@ -105,8 +105,8 @@ public void resetGyro() {
     @Override
     public void periodic() {
         currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
-                frontLeft.getSwerveModulePosition(), frontRight.getSwerveModulePosition(),
-                backLeft.getSwerveModulePosition(), backRight.getSwerveModulePosition()
+                frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                backLeft.getModulePosition(), backRight.getModulePosition()
         });
     }
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 10ba995..6c35888 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -138,7 +138,7 @@ public void resetRelativeEncoder() {
         turnMotor.setPosition(getAbsoluteAngle());
     }
 
-    public SwerveModulePosition getSwerveModulePosition() {
+    public SwerveModulePosition getModulePosition() {
         return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(turnMotor.getPosition()));
     }
 

From 7151a6298d55aaf534e7b1d2dfdaba2d8ff3c9b0 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:48:56 +0200
Subject: [PATCH 133/158] currentPose not static

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9c94d8b..6de8dda 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -36,7 +36,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
-    private static Pose2d currentPose;
+    private Pose2d currentPose;
 
     private static Drivetrain instance;
 

From 7920f2043b2abdc61ddc810c5a900fb15d0fb5f7 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 1 Feb 2025 12:49:46 +0200
Subject: [PATCH 134/158] periodic below constructor

---
 .../java/frc/robot/subsystems/Drivetrain.java    | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 6de8dda..2416e04 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -66,6 +66,14 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         configureDashboard();
     }
 
+    @Override
+    public void periodic() {
+        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
+                frontLeft.getModulePosition(), frontRight.getModulePosition(),
+                backLeft.getModulePosition(), backRight.getModulePosition()
+        });
+    }
+
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
                       boolean usePID) {
         ChassisSpeeds speeds;
@@ -102,14 +110,6 @@ public void resetGyro() {
         gyro.reset();
     }
 
-    @Override
-    public void periodic() {
-        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
-                frontLeft.getModulePosition(), frontRight.getModulePosition(),
-                backLeft.getModulePosition(), backRight.getModulePosition()
-        });
-    }
-
     @Override
     public void configureDashboard() {
         namespace.putNumber("gyro yaw", gyro::getAngle);

From 84729d85ed4eb51f8120068b1ae2cac69e84ba5c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 22 Jan 2025 11:54:30 +0200
Subject: [PATCH 135/158] committing just so I can make changes in Swerve

---
 src/main/deploy/pathplanner/navgrid.json      |   1 +
 .../pathplanner/paths/Example Path.path       |   4 +-
 .../deploy/pathplanner/paths/New Path.path    | 107 ++----------------
 3 files changed, 13 insertions(+), 99 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/navgrid.json

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
index c489599..6730296 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 4.0,
-    "rotation": 107.02052561148976
+    "rotation": 107.02052561148975
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 3.0,
-    "rotation": 107.02052561148976
+    "rotation": -135.0
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index c6f8f92..5cc35f8 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -8,117 +8,30 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.732007133527982,
-        "y": 5.5498307973727705
+        "x": 7.921212121212121,
+        "y": 5.641098484848485
       },
       "isLocked": false,
       "linkedName": "blop"
     },
     {
       "anchor": {
-        "x": 4.184545836832152,
-        "y": 6.349747687125728
+        "x": 4.0,
+        "y": 6.0
       },
       "prevControl": {
-        "x": 6.258429694021986,
-        "y": 7.073495378617061
-      },
-      "nextControl": {
-        "x": 2.380660337304553,
-        "y": 5.720224504585095
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 0.6494533221139411,
-        "y": 2.659671993273115
-      },
-      "prevControl": {
-        "x": 0.7369234423513376,
-        "y": 1.3606143884005357
-      },
-      "nextControl": {
-        "x": 0.5171324068913985,
-        "y": 4.62482867462925
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.2177249789684408,
-        "y": 6.585912531540567
-      },
-      "prevControl": {
-        "x": 2.066972297149061,
-        "y": 6.499392193224025
-      },
-      "nextControl": {
-        "x": 0.8397997012877723,
-        "y": 6.624415123404331
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.284146341457928,
-        "y": 4.512089991591029
-      },
-      "prevControl": {
-        "x": 0.7030640857270156,
-        "y": 5.380354936863563
-      },
-      "nextControl": {
-        "x": 1.8652285971888403,
-        "y": 3.6438250463184954
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.583095037846931,
-        "y": 3.1615222876366693
-      },
-      "prevControl": {
-        "x": 4.224161817517886,
-        "y": 3.4026736669775826
+        "x": 5.199763257575757,
+        "y": 7.356297348484849
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.1,
-      "rotationDegrees": -55.59972476898146
-    }
-  ],
+  "rotationTargets": [],
   "constraintZones": [],
-  "pointTowardsZones": [
-    {
-      "fieldPosition": {
-        "x": 0.4,
-        "y": 5.5
-      },
-      "rotationOffset": 180.0,
-      "minWaypointRelativePos": 0.5697674418604655,
-      "maxWaypointRelativePos": 1.365116279069768,
-      "name": "Point Towards Zone"
-    }
-  ],
-  "eventMarkers": [
-    {
-      "name": "",
-      "waypointRelativePos": 1.1,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 5.0,
@@ -129,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 155.02382809380947
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,

From d907901293271a56bdd19560d3893120866dcc5f Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Wed, 29 Jan 2025 16:48:51 +0200
Subject: [PATCH 136/158] added path planner

---
 .../pathplanner/paths/Example Path.path       |   4 +-
 .../deploy/pathplanner/paths/New Path.path    | 107 ++++++++++++++++--
 2 files changed, 99 insertions(+), 12 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
index 6730296..c489599 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 4.0,
-    "rotation": 107.02052561148975
+    "rotation": 107.02052561148976
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 3.0,
-    "rotation": -135.0
+    "rotation": 107.02052561148976
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 5cc35f8..c6f8f92 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -8,30 +8,117 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.921212121212121,
-        "y": 5.641098484848485
+        "x": 5.732007133527982,
+        "y": 5.5498307973727705
       },
       "isLocked": false,
       "linkedName": "blop"
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 4.184545836832152,
+        "y": 6.349747687125728
       },
       "prevControl": {
-        "x": 5.199763257575757,
-        "y": 7.356297348484849
+        "x": 6.258429694021986,
+        "y": 7.073495378617061
+      },
+      "nextControl": {
+        "x": 2.380660337304553,
+        "y": 5.720224504585095
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.6494533221139411,
+        "y": 2.659671993273115
+      },
+      "prevControl": {
+        "x": 0.7369234423513376,
+        "y": 1.3606143884005357
+      },
+      "nextControl": {
+        "x": 0.5171324068913985,
+        "y": 4.62482867462925
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2177249789684408,
+        "y": 6.585912531540567
+      },
+      "prevControl": {
+        "x": 2.066972297149061,
+        "y": 6.499392193224025
+      },
+      "nextControl": {
+        "x": 0.8397997012877723,
+        "y": 6.624415123404331
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.284146341457928,
+        "y": 4.512089991591029
+      },
+      "prevControl": {
+        "x": 0.7030640857270156,
+        "y": 5.380354936863563
+      },
+      "nextControl": {
+        "x": 1.8652285971888403,
+        "y": 3.6438250463184954
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.583095037846931,
+        "y": 3.1615222876366693
+      },
+      "prevControl": {
+        "x": 4.224161817517886,
+        "y": 3.4026736669775826
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1,
+      "rotationDegrees": -55.59972476898146
+    }
+  ],
   "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 5.5
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.5697674418604655,
+      "maxWaypointRelativePos": 1.365116279069768,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "",
+      "waypointRelativePos": 1.1,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 5.0,
@@ -42,7 +129,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 155.02382809380947
   },
   "reversed": false,
   "folder": null,

From 2313d86ddb9a66792ab57bbc301ca87440b7fec5 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 11:27:25 +0200
Subject: [PATCH 137/158] added named commands

---
 .../deploy/pathplanner/autos/New Auto.auto    |  25 ----
 .../pathplanner/paths/Example Path.path       |  54 -------
 .../deploy/pathplanner/paths/New Path.path    | 141 ------------------
 src/main/deploy/pathplanner/settings.json     |   7 +-
 src/main/java/frc/robot/Robot.java            |  13 +-
 .../frc/robot/commands/autonomous/PPAuto.java |   2 +
 6 files changed, 20 insertions(+), 222 deletions(-)
 delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 delete mode 100644 src/main/deploy/pathplanner/paths/Example Path.path
 delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
deleted file mode 100644
index 64497c1..0000000
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ /dev/null
@@ -1,25 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Example Path"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "New Path"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
deleted file mode 100644
index c489599..0000000
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ /dev/null
@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 2.417329545452214,
-        "y": 5.023626893946309
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.6678597876779695,
-        "y": 5.9755388574449535
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.648153409088578,
-        "y": 3.2169507575826723
-      },
-      "prevControl": {
-        "x": 6.48709732984848,
-        "y": 1.358194205188759
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "blop"
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 5.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 4.0,
-    "rotation": 107.02052561148976
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 3.0,
-    "rotation": 107.02052561148976
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
deleted file mode 100644
index c6f8f92..0000000
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ /dev/null
@@ -1,141 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 6.648153409088578,
-        "y": 3.2169507575826723
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 5.732007133527982,
-        "y": 5.5498307973727705
-      },
-      "isLocked": false,
-      "linkedName": "blop"
-    },
-    {
-      "anchor": {
-        "x": 4.184545836832152,
-        "y": 6.349747687125728
-      },
-      "prevControl": {
-        "x": 6.258429694021986,
-        "y": 7.073495378617061
-      },
-      "nextControl": {
-        "x": 2.380660337304553,
-        "y": 5.720224504585095
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 0.6494533221139411,
-        "y": 2.659671993273115
-      },
-      "prevControl": {
-        "x": 0.7369234423513376,
-        "y": 1.3606143884005357
-      },
-      "nextControl": {
-        "x": 0.5171324068913985,
-        "y": 4.62482867462925
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.2177249789684408,
-        "y": 6.585912531540567
-      },
-      "prevControl": {
-        "x": 2.066972297149061,
-        "y": 6.499392193224025
-      },
-      "nextControl": {
-        "x": 0.8397997012877723,
-        "y": 6.624415123404331
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.284146341457928,
-        "y": 4.512089991591029
-      },
-      "prevControl": {
-        "x": 0.7030640857270156,
-        "y": 5.380354936863563
-      },
-      "nextControl": {
-        "x": 1.8652285971888403,
-        "y": 3.6438250463184954
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.583095037846931,
-        "y": 3.1615222876366693
-      },
-      "prevControl": {
-        "x": 4.224161817517886,
-        "y": 3.4026736669775826
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.1,
-      "rotationDegrees": -55.59972476898146
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [
-    {
-      "fieldPosition": {
-        "x": 0.4,
-        "y": 5.5
-      },
-      "rotationOffset": 180.0,
-      "minWaypointRelativePos": 0.5697674418604655,
-      "maxWaypointRelativePos": 1.365116279069768,
-      "name": "Point Towards Zone"
-    }
-  ],
-  "eventMarkers": [
-    {
-      "name": "",
-      "waypointRelativePos": 1.1,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 5.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 155.02382809380947
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 107.02052561148976
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 2708174..1b6b193 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -2,7 +2,12 @@
   "robotWidth": 0.9,
   "robotLength": 0.9,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "mirror best paths",
+    "just rp paths",
+    "best paths",
+    "algae paths"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 5.0,
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index b7cacda..0570254 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,14 +4,25 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class Robot extends TimedRobot {
 
     @Override
     public void robotInit() {
-
+        NamedCommands.registerCommand("ElevateToL4", new InstantCommand());
+        NamedCommands.registerCommand("OuttakeCoralAngle", new InstantCommand());
+        NamedCommands.registerCommand("ReleaseCoral", new InstantCommand());
+        NamedCommands.registerCommand("ElevateToFeeder", new InstantCommand());
+        NamedCommands.registerCommand("IntakeCoralAngle", new InstantCommand());
+        NamedCommands.registerCommand("IntakeCoral", new InstantCommand());
+        NamedCommands.registerCommand("IntakeAlgaeAngle", new InstantCommand());
+        NamedCommands.registerCommand("ElevateToL3", new InstantCommand());
+        NamedCommands.registerCommand("TakeAlgae", new InstantCommand());
+        NamedCommands.registerCommand("PlaceAlgae", new InstantCommand());
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/autonomous/PPAuto.java b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
index 66be6f5..ac618c5 100644
--- a/src/main/java/frc/robot/commands/autonomous/PPAuto.java
+++ b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
@@ -1,5 +1,6 @@
 package frc.robot.commands.autonomous;
 
+import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
 public class PPAuto extends PathPlannerAuto{
@@ -7,4 +8,5 @@ public class PPAuto extends PathPlannerAuto{
     public PPAuto(String autoName) {
         super(autoName);
     }
+
 }

From d9e4f495f70e67fdc55e7ead40b16f9d2bf16268 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 12:07:07 +0200
Subject: [PATCH 138/158] weh

---
 .../java/frc/robot/subsystems/Drivetrain.java | 96 ++++++++++++++-----
 .../frc/robot/subsystems/SwerveModule.java    | 10 ++
 2 files changed, 83 insertions(+), 23 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 2416e04..f122e87 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,19 +1,25 @@
 package frc.robot.subsystems;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.spikes2212.command.DashboardedSubsystem;
 import com.studica.frc.AHRS;
 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.kinematics.*;
+import edu.wpi.first.wpilibj.DriverStation;
 
 public class Drivetrain extends DashboardedSubsystem {
 
     public static final double MAX_SPEED = 4;
-    public static final double MIN_SPEED = 0.05;
+    public static final double MIN_SPEED = 0.4;
     public static final double MAX_TURN_SPEED = 3;
 
-    private static final double TRACK_WIDTH = 0.6;
-    private static final double TRACK_LENGTH = 0.6;
+    private static final double TRACK_WIDTH = -1;
+    private static final double TRACK_LENGTH = -1;
 
     private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
     private static final Translation2d FRONT_LEFT_WHEEL_POSITION =
@@ -27,24 +33,23 @@ public class Drivetrain extends DashboardedSubsystem {
 
     private static final String NAMESPACE_NAME = "drivetrain";
 
+
     private final SwerveModule frontLeft;
     private final SwerveModule frontRight;
     private final SwerveModule backLeft;
     private final SwerveModule backRight;
     private final AHRS gyro;
-
-    private final SwerveDriveOdometry odometry;
     private final SwerveDriveKinematics kinematics;
 
-    private Pose2d currentPose;
-
+    private SwerveModulePosition[] stupidPointlessArray;
+    private final SwerveDriveOdometry odometry;
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {
         if (instance == null) {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
-                    new AHRS(AHRS.NavXComType.kMXP_SPI));
+                    new AHRS(AHRS.NavXComType.kI2C));
         }
         return instance;
     }
@@ -56,22 +61,54 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
-        this.gyro = gyro;
-        currentPose = new Pose2d();
+        stupidPointlessArray = new SwerveModulePosition[] {frontLeft.getPosition(),
+                frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
-        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), new SwerveModulePosition[]{
-                frontLeft.getModulePosition(), frontRight.getModulePosition(),
-                 backLeft.getModulePosition(), backRight.getModulePosition()});
-        configureDashboard();
+        this.gyro = gyro;
+        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), stupidPointlessArray,
+                new Pose2d());
+        RobotConfig config;
+        try{
+            config = RobotConfig.fromGUISettings();
+        } catch (Exception e) {
+            // Handle exception as needed
+            e.printStackTrace();
+            config = null;
+        }
+
+        // Configure AutoBuilder last
+        AutoBuilder.configure(
+                this::getPose2d, // Robot pose supplier
+                this::resetPose2d, // Method to reset odometry (will be called if your auto has a starting pose)
+                this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+                        new PIDConstants(0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(0.01, 0.0, 0.0) // Rotation PID constants
+                ),
+                config, // The robot configuration
+                () -> {
+                    // Boolean supplier that controls when the path will be mirrored for the red alliance
+                    // This will flip the path being followed to the red side of the field.
+                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                    var alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+                },
+                this // Reference to this subsystem to set requirements
+        );
     }
 
     @Override
     public void periodic() {
-        currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
-                frontLeft.getModulePosition(), frontRight.getModulePosition(),
-                backLeft.getModulePosition(), backRight.getModulePosition()
-        });
+        super.periodic();
+        stupidPointlessArray = new SwerveModulePosition[]{frontLeft.getPosition(),
+                frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
+        odometry.update(gyro.getRotation2d(), stupidPointlessArray);
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
@@ -99,19 +136,32 @@ public void stop() {
         backRight.stop();
     }
 
-    public void resetRelativeEncoders() {
+    public void resetRelativeEncoder() {
         frontLeft.resetRelativeEncoder();
         frontRight.resetRelativeEncoder();
         backLeft.resetRelativeEncoder();
         backRight.resetRelativeEncoder();
     }
 
-    public void resetGyro() {
-        gyro.reset();
+    public Pose2d getPose2d() {
+        return odometry.getPoseMeters();
+    }
+
+    public void resetPose2d(Pose2d desiredPose) {
+        odometry.resetPosition(gyro.getRotation2d(), stupidPointlessArray, desiredPose);
+    }
+
+    public ChassisSpeeds getRobotRelativeSpeeds() {
+        return kinematics.toChassisSpeeds(frontLeft.getState(), frontRight.getState(),
+                backLeft.getState(), backRight.getState());
+    }
+
+    public void driveRobotRelative(ChassisSpeeds speeds) {
+        drive(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond,
+                false, true);
     }
 
     @Override
     public void configureDashboard() {
-        namespace.putNumber("gyro yaw", gyro::getAngle);
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 6c35888..e4375f2 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -142,6 +142,16 @@ public SwerveModulePosition getModulePosition() {
         return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(turnMotor.getPosition()));
     }
 
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(driveMotor.getPosition(),
+                Rotation2d.fromDegrees(getAbsoluteAngle()));
+    }
+
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(driveMotor.getVelocity(),
+                Rotation2d.fromDegrees(getAbsoluteAngle()));
+    }
+
     @Override
     public void configureDashboard() {
         namespace.putNumber("absolute angle", this::getAbsoluteAngle);

From ddd0042f710bf21f6522aca63700033e5b0313bb Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:31:09 +0200
Subject: [PATCH 139/158] add empty line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index f122e87..49002c1 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -164,4 +164,4 @@ public void driveRobotRelative(ChassisSpeeds speeds) {
     @Override
     public void configureDashboard() {
     }
-}
\ No newline at end of file
+}

From aece548605e4e179267465063d6108b002c2cf1e Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:32:02 +0200
Subject: [PATCH 140/158] below final fields

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 49002c1..a1eea07 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -41,8 +41,8 @@ public class Drivetrain extends DashboardedSubsystem {
     private final AHRS gyro;
     private final SwerveDriveKinematics kinematics;
 
-    private SwerveModulePosition[] stupidPointlessArray;
     private final SwerveDriveOdometry odometry;
+    private SwerveModulePosition[] stupidPointlessArray;
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {

From 383d716094bfeb29d6f620c3abcb6920f82c7c26 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:32:57 +0200
Subject: [PATCH 141/158] kp is double

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index a1eea07..e26136e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -84,7 +84,7 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
                 this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
                 (speeds, feedforwards) -> driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
                 new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
-                        new PIDConstants(0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(0.0, 0.0, 0.0), // Translation PID constants
                         new PIDConstants(0.01, 0.0, 0.0) // Rotation PID constants
                 ),
                 config, // The robot configuration

From 1d533cf7af7d2d1ab246ab3e656aa0acc93c2d54 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:35:33 +0200
Subject: [PATCH 142/158] filenames are the same as their name

---
 vendordeps/{PathplannerLib-2025.2.2.json => PathplannerLib.json} | 0
 vendordeps/{REVLib-2025.0.2.json => REVLib.json}                 | 0
 2 files changed, 0 insertions(+), 0 deletions(-)
 rename vendordeps/{PathplannerLib-2025.2.2.json => PathplannerLib.json} (100%)
 rename vendordeps/{REVLib-2025.0.2.json => REVLib.json} (100%)

diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib.json
similarity index 100%
rename from vendordeps/PathplannerLib-2025.2.2.json
rename to vendordeps/PathplannerLib.json
diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib.json
similarity index 100%
rename from vendordeps/REVLib-2025.0.2.json
rename to vendordeps/REVLib.json

From 3e785242ede3107aaa1f19026e2f172b97598537 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:50:09 +0200
Subject: [PATCH 143/158] auto files end with blank line

---
 src/main/deploy/pathplanner/autos/algae auto.auto       | 2 +-
 src/main/deploy/pathplanner/autos/best auto.auto        | 2 +-
 src/main/deploy/pathplanner/autos/just rp auto.auto     | 2 +-
 src/main/deploy/pathplanner/autos/mirror best auto.auto | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/algae auto.auto b/src/main/deploy/pathplanner/autos/algae auto.auto
index 9bbef9d..8797fe0 100644
--- a/src/main/deploy/pathplanner/autos/algae auto.auto	
+++ b/src/main/deploy/pathplanner/autos/algae auto.auto	
@@ -84,4 +84,4 @@
   "resetOdom": true,
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/best auto.auto b/src/main/deploy/pathplanner/autos/best auto.auto
index fbf7ad7..29fa3f2 100644
--- a/src/main/deploy/pathplanner/autos/best auto.auto	
+++ b/src/main/deploy/pathplanner/autos/best auto.auto	
@@ -103,4 +103,4 @@
   "resetOdom": true,
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/just rp auto.auto b/src/main/deploy/pathplanner/autos/just rp auto.auto
index 1fcd190..f6a8c59 100644
--- a/src/main/deploy/pathplanner/autos/just rp auto.auto	
+++ b/src/main/deploy/pathplanner/autos/just rp auto.auto	
@@ -41,4 +41,4 @@
   "resetOdom": true,
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/mirror best auto.auto b/src/main/deploy/pathplanner/autos/mirror best auto.auto
index 05f74ad..c4fe86b 100644
--- a/src/main/deploy/pathplanner/autos/mirror best auto.auto	
+++ b/src/main/deploy/pathplanner/autos/mirror best auto.auto	
@@ -103,4 +103,4 @@
   "resetOdom": true,
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}

From 447b8e5f3a521b34a750cd3c51d8a09b4e5b7d74 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:51:57 +0200
Subject: [PATCH 144/158] removed blank line

---
 src/main/java/frc/robot/commands/autonomous/PPAuto.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/autonomous/PPAuto.java b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
index ac618c5..932d2d1 100644
--- a/src/main/java/frc/robot/commands/autonomous/PPAuto.java
+++ b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
@@ -8,5 +8,4 @@ public class PPAuto extends PathPlannerAuto{
     public PPAuto(String autoName) {
         super(autoName);
     }
-
 }

From 531f9ecb81b11e221c545f4dee58814b6c74739d Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:52:34 +0200
Subject: [PATCH 145/158] ctr alt l

---
 src/main/java/frc/robot/commands/autonomous/PPAuto.java | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/autonomous/PPAuto.java b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
index 932d2d1..e6ed33e 100644
--- a/src/main/java/frc/robot/commands/autonomous/PPAuto.java
+++ b/src/main/java/frc/robot/commands/autonomous/PPAuto.java
@@ -1,9 +1,8 @@
 package frc.robot.commands.autonomous;
 
-import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
-public class PPAuto extends PathPlannerAuto{
+public class PPAuto extends PathPlannerAuto {
 
     public PPAuto(String autoName) {
         super(autoName);

From 870ab336dd8fa643af8cf95f9865aca49eb01e91 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:56:09 +0200
Subject: [PATCH 146/158] changed stupid pointless arrays name

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index e26136e..233efc2 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -7,7 +7,6 @@
 import com.spikes2212.command.DashboardedSubsystem;
 import com.studica.frc.AHRS;
 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.kinematics.*;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -42,7 +41,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveKinematics kinematics;
 
     private final SwerveDriveOdometry odometry;
-    private SwerveModulePosition[] stupidPointlessArray;
+    private SwerveModulePosition[] swervePositions;
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {
@@ -61,12 +60,12 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
-        stupidPointlessArray = new SwerveModulePosition[] {frontLeft.getPosition(),
+        swervePositions = new SwerveModulePosition[] {frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         this.gyro = gyro;
-        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), stupidPointlessArray,
+        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), swervePositions,
                 new Pose2d());
         RobotConfig config;
         try{
@@ -106,9 +105,9 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
     @Override
     public void periodic() {
         super.periodic();
-        stupidPointlessArray = new SwerveModulePosition[]{frontLeft.getPosition(),
+        swervePositions = new SwerveModulePosition[]{frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
-        odometry.update(gyro.getRotation2d(), stupidPointlessArray);
+        odometry.update(gyro.getRotation2d(), swervePositions);
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
@@ -148,7 +147,7 @@ public Pose2d getPose2d() {
     }
 
     public void resetPose2d(Pose2d desiredPose) {
-        odometry.resetPosition(gyro.getRotation2d(), stupidPointlessArray, desiredPose);
+        odometry.resetPosition(gyro.getRotation2d(), swervePositions, desiredPose);
     }
 
     public ChassisSpeeds getRobotRelativeSpeeds() {

From 60432556c3f1a859f9738eba50778801e7e5e011 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 14:57:04 +0200
Subject: [PATCH 147/158] correct port to gyro

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 233efc2..73a6c52 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -48,7 +48,7 @@ public static Drivetrain getInstance() {
         if (instance == null) {
             instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
                     SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
-                    new AHRS(AHRS.NavXComType.kI2C));
+                    new AHRS(AHRS.NavXComType.kMXP_SPI));
         }
         return instance;
     }

From 9a75056eeb11c1c2dc6aa94fbf37b539deb8e09e Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 15:15:16 +0200
Subject: [PATCH 148/158] blank line at the end of path files

---
 src/main/deploy/pathplanner/paths/algae path pt1.path       | 2 +-
 src/main/deploy/pathplanner/paths/algae path pt2.path       | 2 +-
 src/main/deploy/pathplanner/paths/algae path pt3.path       | 2 +-
 src/main/deploy/pathplanner/paths/best path pt1.path        | 2 +-
 src/main/deploy/pathplanner/paths/best path pt2.path        | 2 +-
 src/main/deploy/pathplanner/paths/best path pt3.path        | 2 +-
 src/main/deploy/pathplanner/paths/just rp path.path         | 2 +-
 src/main/deploy/pathplanner/paths/mirror best path pt1.path | 2 +-
 src/main/deploy/pathplanner/paths/mirror best path pt2.path | 2 +-
 src/main/deploy/pathplanner/paths/mirror best path pt3.path | 2 +-
 10 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/algae path pt1.path b/src/main/deploy/pathplanner/paths/algae path pt1.path
index bdb0eab..71219ef 100644
--- a/src/main/deploy/pathplanner/paths/algae path pt1.path	
+++ b/src/main/deploy/pathplanner/paths/algae path pt1.path	
@@ -51,4 +51,4 @@
     "rotation": 180.0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/algae path pt2.path b/src/main/deploy/pathplanner/paths/algae path pt2.path
index 521b6c5..f3d360a 100644
--- a/src/main/deploy/pathplanner/paths/algae path pt2.path	
+++ b/src/main/deploy/pathplanner/paths/algae path pt2.path	
@@ -51,4 +51,4 @@
     "rotation": 180.0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/algae path pt3.path b/src/main/deploy/pathplanner/paths/algae path pt3.path
index 7fa558a..cf91a1a 100644
--- a/src/main/deploy/pathplanner/paths/algae path pt3.path	
+++ b/src/main/deploy/pathplanner/paths/algae path pt3.path	
@@ -51,4 +51,4 @@
     "rotation": 180.0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/best path pt1.path b/src/main/deploy/pathplanner/paths/best path pt1.path
index aba8568..ddafb3b 100644
--- a/src/main/deploy/pathplanner/paths/best path pt1.path	
+++ b/src/main/deploy/pathplanner/paths/best path pt1.path	
@@ -51,4 +51,4 @@
     "rotation": -179.1574757392596
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/best path pt2.path b/src/main/deploy/pathplanner/paths/best path pt2.path
index 9f02d90..53be9f7 100644
--- a/src/main/deploy/pathplanner/paths/best path pt2.path	
+++ b/src/main/deploy/pathplanner/paths/best path pt2.path	
@@ -51,4 +51,4 @@
     "rotation": 123.42775135269798
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/best path pt3.path b/src/main/deploy/pathplanner/paths/best path pt3.path
index c0dea53..f769d10 100644
--- a/src/main/deploy/pathplanner/paths/best path pt3.path	
+++ b/src/main/deploy/pathplanner/paths/best path pt3.path	
@@ -51,4 +51,4 @@
     "rotation": -128.29016319224309
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/just rp path.path b/src/main/deploy/pathplanner/paths/just rp path.path
index cde3a4c..f995b9e 100644
--- a/src/main/deploy/pathplanner/paths/just rp path.path	
+++ b/src/main/deploy/pathplanner/paths/just rp path.path	
@@ -51,4 +51,4 @@
     "rotation": 180.0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/mirror best path pt1.path b/src/main/deploy/pathplanner/paths/mirror best path pt1.path
index c127582..69b0d11 100644
--- a/src/main/deploy/pathplanner/paths/mirror best path pt1.path	
+++ b/src/main/deploy/pathplanner/paths/mirror best path pt1.path	
@@ -51,4 +51,4 @@
     "rotation": -179.28384005452952
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/mirror best path pt2.path b/src/main/deploy/pathplanner/paths/mirror best path pt2.path
index 8ba2d3d..6d99c60 100644
--- a/src/main/deploy/pathplanner/paths/mirror best path pt2.path	
+++ b/src/main/deploy/pathplanner/paths/mirror best path pt2.path	
@@ -51,4 +51,4 @@
     "rotation": -122.82854179141262
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/mirror best path pt3.path b/src/main/deploy/pathplanner/paths/mirror best path pt3.path
index bb278e5..82f7d09 100644
--- a/src/main/deploy/pathplanner/paths/mirror best path pt3.path	
+++ b/src/main/deploy/pathplanner/paths/mirror best path pt3.path	
@@ -51,4 +51,4 @@
     "rotation": 125.71995431229035
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}

From bb202d732c467292507a46e852baaa4f661b55cd Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 15:15:46 +0200
Subject: [PATCH 149/158] added blank line

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 73a6c52..f526a4e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -42,6 +42,7 @@ public class Drivetrain extends DashboardedSubsystem {
 
     private final SwerveDriveOdometry odometry;
     private SwerveModulePosition[] swervePositions;
+
     private static Drivetrain instance;
 
     public static Drivetrain getInstance() {

From 04471c4ca5f3b43078ab70db32f5cf7635db9741 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 15:19:24 +0200
Subject: [PATCH 150/158] added blank line

---
 src/main/deploy/pathplanner/navgrid.json  | 1657 ++++++++++++++++++++-
 src/main/deploy/pathplanner/settings.json |    2 +-
 2 files changed, 1657 insertions(+), 2 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 23e0db9..bbd9194 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1,1656 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{
+  "field_size": {
+    "x": 17.548,
+    "y": 8.052
+  },
+  "nodeSizeMeters": 0.3,
+  "grid": [
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ]
+  ]
+}
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 1b6b193..4a55cb4 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -34,4 +34,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
\ No newline at end of file
+}

From e524b1d5ea5a43cf6183e06e769d78a3e6a29bd7 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 15:27:53 +0200
Subject: [PATCH 151/158] changed stupid pointless array name 2.0

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index f526a4e..40d6b39 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -41,7 +41,7 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveKinematics kinematics;
 
     private final SwerveDriveOdometry odometry;
-    private SwerveModulePosition[] swervePositions;
+    private SwerveModulePosition[] swerveModulePositions;
 
     private static Drivetrain instance;
 
@@ -61,12 +61,12 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
-        swervePositions = new SwerveModulePosition[] {frontLeft.getPosition(),
+        swerveModulePositions = new SwerveModulePosition[] {frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         this.gyro = gyro;
-        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), swervePositions,
+        odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), swerveModulePositions,
                 new Pose2d());
         RobotConfig config;
         try{
@@ -106,9 +106,9 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
     @Override
     public void periodic() {
         super.periodic();
-        swervePositions = new SwerveModulePosition[]{frontLeft.getPosition(),
+        swerveModulePositions = new SwerveModulePosition[]{frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
-        odometry.update(gyro.getRotation2d(), swervePositions);
+        odometry.update(gyro.getRotation2d(), swerveModulePositions);
     }
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
@@ -148,7 +148,7 @@ public Pose2d getPose2d() {
     }
 
     public void resetPose2d(Pose2d desiredPose) {
-        odometry.resetPosition(gyro.getRotation2d(), swervePositions, desiredPose);
+        odometry.resetPosition(gyro.getRotation2d(), swerveModulePositions, desiredPose);
     }
 
     public ChassisSpeeds getRobotRelativeSpeeds() {

From 6edea84801c4f050d374fc322d344b323945286c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 15:30:10 +0200
Subject: [PATCH 152/158] initialized values correct order

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 40d6b39..d540dc4 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -61,11 +61,11 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.frontRight = frontRight;
         this.backLeft = backLeft;
         this.backRight = backRight;
+        this.gyro = gyro;
         swerveModulePositions = new SwerveModulePosition[] {frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
-        this.gyro = gyro;
         odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), swerveModulePositions,
                 new Pose2d());
         RobotConfig config;

From 5877bc28682723fc6c51ce323bb81ca3cefa8b3d Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Sat, 8 Feb 2025 15:36:25 +0200
Subject: [PATCH 153/158] ctr alt l

---
 src/main/java/frc/robot/commands/Drive.java        | 4 ++--
 src/main/java/frc/robot/subsystems/Drivetrain.java | 7 +++----
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index f394ebf..31c14df 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -8,8 +8,8 @@
 
 public class Drive extends Command {
 
-    private final double DRIVE_ACCELERATION_LIMIT = 2;
-    private final double TURN_ACCELERATION_LIMIT = 4;
+    private static final double DRIVE_ACCELERATION_LIMIT = 2;
+    private static final double TURN_ACCELERATION_LIMIT = 4;
 
     private final Drivetrain drivetrain;
     private final Supplier<Double> xSpeed;
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index d540dc4..a4b0a95 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -62,14 +62,14 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
         this.backLeft = backLeft;
         this.backRight = backRight;
         this.gyro = gyro;
-        swerveModulePositions = new SwerveModulePosition[] {frontLeft.getPosition(),
+        swerveModulePositions = new SwerveModulePosition[]{frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
                 FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
         odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), swerveModulePositions,
                 new Pose2d());
         RobotConfig config;
-        try{
+        try {
             config = RobotConfig.fromGUISettings();
         } catch (Exception e) {
             // Handle exception as needed
@@ -117,8 +117,7 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
         if (fieldRelative) {
             speeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed,
                     gyro.getRotation2d());
-        }
-        else {
+        } else {
             speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
         }
         SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, CENTER_OF_ROBOT);

From 9312d0b7992d5e7ecfd6f062341d9aa1eca8e116 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 13 Feb 2025 11:08:39 +0200
Subject: [PATCH 154/158] ctr alt l

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5c7d316..64840ff 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -112,8 +112,8 @@ public void periodic() {
         swerveModulePositions = new SwerveModulePosition[]{frontLeft.getPosition(),
                 frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()};
         currentPose = odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] {
-                frontLeft.getModulePosition(), frontRight.getModulePosition(),
-                backLeft.getModulePosition(), backRight.getModulePosition()
+                frontLeft.getPosition(), frontRight.getPosition(),
+                backLeft.getPosition(), backRight.getPosition()
         });
     }
 

From 98cff70db46dda57f5733d0831e423d86d523d1c Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 13 Feb 2025 11:09:46 +0200
Subject: [PATCH 155/158] ctr alt l

---
 vendordeps/PathplannerLib-2025.2.2.json | 38 -------------
 vendordeps/REVLib-2025.0.2.json         | 71 -------------------------
 2 files changed, 109 deletions(-)
 delete mode 100644 vendordeps/PathplannerLib-2025.2.2.json
 delete mode 100644 vendordeps/REVLib-2025.0.2.json

diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.2.2.json
deleted file mode 100644
index c0f7c11..0000000
--- a/vendordeps/PathplannerLib-2025.2.2.json
+++ /dev/null
@@ -1,38 +0,0 @@
-{
-  "fileName": "PathplannerLib.json",
-  "name": "PathplannerLib",
-  "version": "2025.2.2",
-  "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
-  "frcYear": "2025",
-  "mavenUrls": [
-    "https://3015rangerrobotics.github.io/pathplannerlib/repo"
-  ],
-  "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
-  "javaDependencies": [
-    {
-      "groupId": "com.pathplanner.lib",
-      "artifactId": "PathplannerLib-java",
-      "version": "2025.2.2"
-    }
-  ],
-  "jniDependencies": [],
-  "cppDependencies": [
-    {
-      "groupId": "com.pathplanner.lib",
-      "artifactId": "PathplannerLib-cpp",
-      "version": "2025.2.2",
-      "libName": "PathplannerLib",
-      "headerClassifier": "headers",
-      "sharedLibrary": false,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "linuxx86-64",
-        "osxuniversal",
-        "linuxathena",
-        "linuxarm32",
-        "linuxarm64"
-      ]
-    }
-  ]
-}
diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib-2025.0.2.json
deleted file mode 100644
index 30ea8de..0000000
--- a/vendordeps/REVLib-2025.0.2.json
+++ /dev/null
@@ -1,71 +0,0 @@
-{
-    "fileName": "REVLib.json",
-    "name": "REVLib",
-    "version": "2025.0.2",
-    "frcYear": "2025",
-    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
-    "mavenUrls": [
-        "https://maven.revrobotics.com/"
-    ],
-    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json",
-    "javaDependencies": [
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-java",
-            "version": "2025.0.2"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
-            "skipInvalidPlatforms": true,
-            "isJar": false,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-cpp",
-            "version": "2025.0.2",
-            "libName": "REVLib",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        },
-        {
-            "groupId": "com.revrobotics.frc",
-            "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
-            "libName": "REVLibDriver",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxarm64",
-                "linuxx86-64",
-                "linuxathena",
-                "linuxarm32",
-                "osxuniversal"
-            ]
-        }
-    ]
-}

From c325e7ee9921e91ffcd527f740884fd7b8af95a8 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 13 Feb 2025 11:25:29 +0200
Subject: [PATCH 156/158] safd

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 64840ff..c7942c5 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -119,7 +119,7 @@ public void periodic() {
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
                       boolean usePID) {
-        ChassisSpeeds speeds;
+        ChassisSpeeds speeds
         if (fieldRelative) {
             speeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed,
                     gyro.getRotation2d());

From 828a37ed32531d695df4d25ff231d2f06c2111d9 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 13 Feb 2025 11:25:43 +0200
Subject: [PATCH 157/158] safd

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index c7942c5..64840ff 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -119,7 +119,7 @@ public void periodic() {
 
     public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
                       boolean usePID) {
-        ChassisSpeeds speeds
+        ChassisSpeeds speeds;
         if (fieldRelative) {
             speeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed,
                     gyro.getRotation2d());

From 486311e724ed3955ee494d6e76c6d6b5f7f87295 Mon Sep 17 00:00:00 2001
From: GIL <eingar.gil@gmail.com>
Date: Thu, 13 Feb 2025 11:35:56 +0200
Subject: [PATCH 158/158] ctrl alt l

---
 src/main/java/frc/robot/RobotMap.java | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index e6ab479..dd215b6 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -3,7 +3,7 @@
 public class RobotMap {
 
     public interface CAN {
-      
+
         int FRONT_LEFT_DRIVE_TALON_FX = 1;
         int FRONT_RIGHT_DRIVE_TALON_FX = 4;
         int BACK_LEFT_DRIVE_TALON_FX = 7;
@@ -19,14 +19,14 @@ public interface CAN {
         int BACK_LEFT_ABSOLUTE_ENCODER = 9;
         int BACK_RIGHT_ABSOLUTE_ENCODER = 12;
 
-      int ELEVATOR_MASTER_SPARK = -1;
-      int ELEVATOR_SLAVE_SPARK = -1;
-      int ALGAE_JOINT_SPARK = -1;
-      int CORAL_JOINT_SPARK = -1;
-      int GRIPPER_TALON = -1;
-      int STORAGE_SPARK = -1;
+        int ELEVATOR_MASTER_SPARK = -1;
+        int ELEVATOR_SLAVE_SPARK = -1;
+        int ALGAE_JOINT_SPARK = -1;
+        int CORAL_JOINT_SPARK = -1;
+        int GRIPPER_TALON = -1;
+        int STORAGE_SPARK = -1;
     }
-  
+
     public interface DIO {
 
         int ELEVATOR_TOP_LIMIT = -1;