From ca2019549e81f997daf03cbf0337bb656ec0a194 Mon Sep 17 00:00:00 2001 From: iefomit Date: Thu, 2 Apr 2026 22:34:08 -0700 Subject: [PATCH] add sample limit --- .../java/frc/robot/controls/PS5ControllerDriverConfig.java | 5 ----- .../java/frc/robot/subsystems/drivetrain/Drivetrain.java | 7 +++++++ 2 files changed, 7 insertions(+), 5 deletions(-) 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]; -- 2.39.5