add sample limit

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 98e6f1033f9b8cf85a29e3cd0733d753fbc4039a..3349c84d5f5e387db72dee797e3830aa2d0305f4 100644 (file)
--- 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 55ac197163d2861c9b8b4eabb9191ded7a9d8d00..550765ef19bc057386ef56fa51413eda71a8ec7c 100644 (file)
--- 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];