From: iefomit <timofei.stem@gmail.com>
Date: Fri, 3 Apr 2026 05:34:08 +0000 (-0700)
Subject: add sample limit
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=ca2019549e81f997daf03cbf0337bb656ec0a194;p=FRC2026.git

add sample limit
---

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 98e6f10..3349c84 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -4,19 +4,14 @@ import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.robot.Robot;
 import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
-import frc.robot.commands.led_comm.DefenseLightsCommand;
-import frc.robot.commands.led_comm.LEDDefaultCommand;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.Intake.Intake;
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 55ac197..550765e 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -210,6 +210,13 @@ public class Drivetrain extends SubsystemBase {
             positions[i] = modules[i].getOdometryPositions();
             sampleCount = Math.min(sampleCount, positions[i].length);
         }
+
+        // cap samples per cycle, more gives little benefit
+        final int MAX_SAMPLES_PER_CYCLE = 10;
+        if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
+            sampleCount = MAX_SAMPLES_PER_CYCLE;
+        }
+
         for (int i = 0; i < sampleCount; i++) {
             // Read wheel positions and deltas from each module
             SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];