From aff6b1ea6bfe76efb7d1bbd23164d442feae9c70 Mon Sep 17 00:00:00 2001 From: darkpotatoo Date: Tue, 11 Nov 2025 16:24:35 -0500 Subject: [PATCH 1/7] rename, for now --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../robot/subsystems/drive/GyroIONavX.java | 3 +-- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 2 +- .../drive/PhoenixOdometryThread.java | 21 +++++++++---------- ...iveSubsystem.java => SwerveDriveReal.java} | 4 ++-- .../subsystems/drive/SwerveDriveSim.java | 2 +- 7 files changed, 18 insertions(+), 20 deletions(-) rename src/main/java/frc/robot/subsystems/drive/{SwerveDriveSubsystem.java => SwerveDriveReal.java} (99%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e572a0b..3e3f554 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,7 +111,7 @@ private SwerveDriveIO configureDrive() { return switch (Constants.CURRENT_MODE) { case REAL -> // Real robot, instantiate hardware IO implementations - new SwerveDriveSubsystem( + new SwerveDriveReal( new GyroIOPigeon2(), new ModuleIOTalonFX(TunerConstants.FrontLeft), new ModuleIOTalonFX(TunerConstants.FrontRight), @@ -122,7 +122,7 @@ private SwerveDriveIO configureDrive() { new SwerveDriveSim(); default -> // Replayed robot, disable IO implementations - new SwerveDriveSubsystem( + new SwerveDriveReal( new GyroIO() {}, new ModuleIO() {}, new ModuleIO() {}, diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 405fd4a..b5d583a 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -8,8 +8,7 @@ /** IO implementation for NavX. */ public class GyroIONavX implements GyroIO { - private final AHRS navX = - new AHRS(NavXComType.kMXP_SPI, (byte) SwerveDriveSubsystem.ODOMETRY_FREQUENCY); + private final AHRS navX = new AHRS(NavXComType.kMXP_SPI, (byte) SwerveDriveIO.ODOMETRY_FREQUENCY); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 7a4bf81..eb6645e 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -26,7 +26,7 @@ public class GyroIOPigeon2 implements GyroIO { public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(SwerveDriveSubsystem.ODOMETRY_FREQUENCY); + yaw.setUpdateFrequency(SwerveDriveIO.ODOMETRY_FREQUENCY); yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 98ba30d..8dbc9d0 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -157,7 +157,7 @@ public ModuleIOTalonFX( // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( - SwerveDriveSubsystem.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + SwerveDriveIO.ODOMETRY_FREQUENCY, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index dcfaf12..7f908c9 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -58,7 +58,7 @@ public synchronized void start() { public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); - SwerveDriveSubsystem.ODOMETRY_LOCK.lock(); + SwerveDriveReal.ODOMETRY_LOCK.lock(); try { BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); @@ -67,7 +67,7 @@ public Queue registerSignal(StatusSignal signal) { phoenixQueues.add(queue); } finally { signalsLock.unlock(); - SwerveDriveSubsystem.ODOMETRY_LOCK.unlock(); + SwerveDriveReal.ODOMETRY_LOCK.unlock(); } return queue; } @@ -76,13 +76,13 @@ public Queue registerSignal(StatusSignal signal) { public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); - SwerveDriveSubsystem.ODOMETRY_LOCK.lock(); + SwerveDriveReal.ODOMETRY_LOCK.lock(); try { genericSignals.add(signal); genericQueues.add(queue); } finally { signalsLock.unlock(); - SwerveDriveSubsystem.ODOMETRY_LOCK.unlock(); + SwerveDriveReal.ODOMETRY_LOCK.unlock(); } return queue; } @@ -90,11 +90,11 @@ public Queue registerSignal(DoubleSupplier signal) { /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); - SwerveDriveSubsystem.ODOMETRY_LOCK.lock(); + SwerveDriveReal.ODOMETRY_LOCK.lock(); try { timestampQueues.add(queue); } finally { - SwerveDriveSubsystem.ODOMETRY_LOCK.unlock(); + SwerveDriveReal.ODOMETRY_LOCK.unlock(); } return queue; } @@ -106,13 +106,12 @@ public void run() { signalsLock.lock(); try { if (IS_CANFD && phoenixSignals.length > 0) { - BaseStatusSignal.waitForAll( - 2.0 / SwerveDriveSubsystem.ODOMETRY_FREQUENCY, phoenixSignals); + BaseStatusSignal.waitForAll(2.0 / SwerveDriveReal.ODOMETRY_FREQUENCY, phoenixSignals); } else { // "waitForAll" does not support blocking on multiple signals with a bus // that is not CAN FD, regardless of Pro licensing. No reasoning for this // behavior is provided by the documentation. - Thread.sleep((long) (1000.0 / SwerveDriveSubsystem.ODOMETRY_FREQUENCY)); + Thread.sleep((long) (1000.0 / SwerveDriveReal.ODOMETRY_FREQUENCY)); if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); } } catch (InterruptedException e) { @@ -123,7 +122,7 @@ public void run() { } // Save new data to queues - SwerveDriveSubsystem.ODOMETRY_LOCK.lock(); + SwerveDriveReal.ODOMETRY_LOCK.lock(); try { // Sample timestamp is current FPGA time minus average CAN latency // Default timestamps from Phoenix are NOT compatible with @@ -148,7 +147,7 @@ public void run() { timestampQueue.offer(timestamp); } } finally { - SwerveDriveSubsystem.ODOMETRY_LOCK.unlock(); + SwerveDriveReal.ODOMETRY_LOCK.unlock(); } } } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java similarity index 99% rename from src/main/java/frc/robot/subsystems/drive/SwerveDriveSubsystem.java rename to src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java index 9ac223a..78699b6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java @@ -34,7 +34,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class SwerveDriveSubsystem implements SwerveDriveIO { +public class SwerveDriveReal implements SwerveDriveIO { private final SysIdRoutine sysId; private final GyroIO gyroIO; @@ -43,7 +43,7 @@ public class SwerveDriveSubsystem implements SwerveDriveIO { new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); private final Field2d field2d = new Field2d(); - public SwerveDriveSubsystem( + public SwerveDriveReal( GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java b/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java index 2326b36..410a4cb 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java @@ -50,7 +50,7 @@ public SwerveDriveSim() { .withTrackLengthTrackWidth(Inches.of(22), Inches.of(22)) .withBumperSize(Inches.of(34), Inches.of(34)) .withRobotMass(Pounds.of(125)) - .withCustomModuleTranslations(SwerveDriveSubsystem.getModuleTranslations()); + .withCustomModuleTranslations(SwerveDriveIO.getModuleTranslations()); driveSimulaton = new SelfControlledSwerveDriveSimulation( From 08ae1b485109bfac1b005cb551da99bb9fe24584 Mon Sep 17 00:00:00 2001 From: darkpotatoo Date: Sun, 30 Nov 2025 21:04:59 -0500 Subject: [PATCH 2/7] remove duplicated configuring --- .../robot/subsystems/drive/SwerveDriveIO.java | 33 +++++++++++++ .../subsystems/drive/SwerveDriveReal.java | 30 ++---------- .../subsystems/drive/SwerveDriveSim.java | 48 ++----------------- 3 files changed, 39 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDriveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveDriveIO.java index e91a21b..784203a 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDriveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDriveIO.java @@ -3,8 +3,13 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import com.ctre.phoenix6.CANBus; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,14 +23,17 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; +import frc.robot.util.LocalADStarAK; import frc.robot.util.TimestampedVisionUpdate; import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; public interface SwerveDriveIO extends Subsystem { @@ -87,6 +95,31 @@ static Translation2d[] getModuleTranslations() { }; } + default void configure() { + + AutoBuilder.configure( + this::getPose, + this::setPose, + this::getChassisSpeeds, + this::runVelocity, + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + PP_CONFIG, + () -> + DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + == DriverStation.Alliance + .Red, // this is correct, model all trajectories for blue side in path planner + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + activePath -> + Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); + PathPlannerLogging.setLogTargetPoseCallback( + targetPose -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); + } + + ChassisSpeeds getChassisSpeeds(); + Transform2d getVelocity(); @Override diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java b/src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java index 78699b6..d5327eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDriveReal.java @@ -3,11 +3,6 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; @@ -28,7 +23,6 @@ import frc.robot.Constants; import frc.robot.Constants.Mode; import frc.robot.generated.TunerConstants; -import frc.robot.util.LocalADStarAK; import frc.robot.util.TimestampedVisionUpdate; import java.util.List; import org.littletonrobotics.junction.AutoLogOutput; @@ -61,26 +55,7 @@ public SwerveDriveReal( // Start odometry thread PhoenixOdometryThread.getInstance().start(); - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( - this::getPose, - this::setPose, - this::getChassisSpeeds, - this::runVelocity, - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, - () -> - DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) - == DriverStation.Alliance - .Red, // this is correct, model all trajectories for blue side in path planner - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - activePath -> - Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); - PathPlannerLogging.setLogTargetPoseCallback( - targetPose -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); + configure(); // Configure AutoBuilder and PathPlanner // Configure SysId sysId = @@ -260,8 +235,9 @@ private SwerveModulePosition[] getModulePositions() { } /** Returns the measured chassis speeds of the robot. */ + @Override @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - private ChassisSpeeds getChassisSpeeds() { + public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java b/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java index 410a4cb..bc64b8f 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDriveSim.java @@ -2,11 +2,6 @@ import static edu.wpi.first.units.Units.*; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -16,16 +11,11 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; -import frc.robot.util.LocalADStarAK; import frc.robot.util.TimestampedVisionUpdate; import java.util.List; import org.ironmaple.simulation.SimulatedArena; @@ -61,43 +51,11 @@ public SwerveDriveSim() { field2d = new Field2d(); SmartDashboard.putData("Simulation Field", field2d); - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( - this::getPose, - this::setPose, - this::getChassisSpeeds, - this::runVelocity, - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, - () -> - DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) - == DriverStation.Alliance - .Red, // this is correct, model all trajectories for blue side in path planner - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - activePath -> { - Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0])); - if (activePath.isEmpty()) return; - Trajectory trajectory = - TrajectoryGenerator.generateTrajectory( - activePath.get(0), - activePath.stream() - .skip(1) - .limit(activePath.size() - (long) 2) - .map(Pose2d::getTranslation) - .toList(), - activePath.get(activePath.size() - 1), - new TrajectoryConfig(getMaxLinearSpeedMetersPerSec(), 4.2)); - - field2d.getObject("traj").setTrajectory(trajectory); - }); - PathPlannerLogging.setLogTargetPoseCallback( - targetPose -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); + configure(); // Configure AutoBuilder and PathPlanner } - private ChassisSpeeds getChassisSpeeds() { + @Override + public ChassisSpeeds getChassisSpeeds() { return SwerveDriveIO.kinematics.toChassisSpeeds(driveSimulaton.getMeasuredStates()); } From 8a290a857c707fe77c561241bec56bca7048d835 Mon Sep 17 00:00:00 2001 From: darkpotatoo Date: Sun, 7 Dec 2025 13:53:26 -0500 Subject: [PATCH 3/7] tested --- .../frc/robot/generated/TunerConstants.java | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 621ee7b..b229382 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -20,6 +20,8 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; +// TODO: Slip Current, Steer/Drive Current Limits, Steer Gains + // Generated using https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -37,9 +39,9 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(50) // less force on steering motor, less jerky + .withKP(100) // less force on steering motor, less jerky .withKI(0) - .withKD(0.5) + .withKD(0.2) .withKS(0.1) .withKV(1.5) .withKA(0) @@ -73,7 +75,14 @@ public class TunerConstants { private static final Current kSlipCurrent = Amps.of(100.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(40) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimit(40) + .withStatorCurrentLimitEnable(true)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -82,7 +91,9 @@ public class TunerConstants { // low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(40)) + .withSupplyCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; From b22a12262fbf998d0d03998b14a7f934e4a69ba1 Mon Sep 17 00:00:00 2001 From: darkpotatoo Date: Mon, 22 Dec 2025 09:08:11 -0500 Subject: [PATCH 4/7] remove the highly unnecessary suppliers --- .../frc/robot/commands/DriveCommands.java | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 0039e3b..de5f347 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -329,21 +329,16 @@ public static Command goToTransformWithPathFinder( * using the provided control points. * * @param drive The drive subsystem - * @param startSupplier Pose to start at - * @param endSupplier Pose to end at + * @param start Pose to start at + * @param end Pose to end at * @param controls Control points */ public static Command followCurve( - SwerveDriveIO drive, - Supplier startSupplier, - Supplier endSupplier, - Supplier controls) { + SwerveDriveIO drive, Pose2d start, Pose2d end, Pose2d... controls) { - Pose2d start = startSupplier.get(); - Pose2d end = endSupplier.get(); List pts = new ArrayList<>(); pts.add(start.getTranslation()); - for (Pose2d pose : controls.get()) { + for (Pose2d pose : controls) { pts.add(pose.getTranslation()); } pts.add(end.getTranslation()); @@ -395,7 +390,7 @@ public static Command followCurve( sampled[i] = new Pose2d(pos, rot); } - return followPoses(drive, () -> sampled); + return followPoses(drive, sampled); } /** @@ -403,10 +398,12 @@ public static Command followCurve( * the provided poses. * * @param drive The drive subsystem - * @param pointArraySupplier {@code Pose2d} points to construct a path out of + * @param points {@code Pose2d} points to construct a path out of */ - public static Command followPoses(SwerveDriveIO drive, Supplier pointArraySupplier) { - Pose2d[] points = pointArraySupplier.get(); + public static Command followPoses(SwerveDriveIO drive, Pose2d... points) { + if (points == null || points.length == 0) { + return new InstantCommand(() -> {}); + } List waypoints = PathPlannerPath.waypointsFromPoses(points); PathPlannerPath path = new PathPlannerPath( @@ -431,11 +428,13 @@ public static Command followPoses(SwerveDriveIO drive, Supplier pointA * @param drive The drive subsystem * @param transitionVelocity The speed in m/s that should be maintained from the initial pathing * when starting to follow the actual path - * @param pointArraySupplier {@code Pose2d} points to construct a path out of + * @param points {@code Pose2d} points to construct a path out of */ public static Command followPoses( - SwerveDriveIO drive, double transitionVelocity, Supplier pointArraySupplier) { - Pose2d[] points = pointArraySupplier.get(); + SwerveDriveIO drive, double transitionVelocity, Pose2d... points) { + if (points == null || points.length == 0) { + return new InstantCommand(() -> {}); + } List waypoints = PathPlannerPath.waypointsFromPoses(points); PathPlannerPath path = new PathPlannerPath( From bff559f4b8cdfe47c6d89891a88bfb0a8fa0a00d Mon Sep 17 00:00:00 2001 From: darkpotatoo Date: Mon, 22 Dec 2025 09:09:27 -0500 Subject: [PATCH 5/7] revert steer pid --- src/main/java/frc/robot/generated/TunerConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b229382..5da546d 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -39,9 +39,9 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100) // less force on steering motor, less jerky + .withKP(50) // less force on steering motor, less jerky .withKI(0) - .withKD(0.2) + .withKD(0.5) .withKS(0.1) .withKV(1.5) .withKA(0) From cb24c1362a1a9d00d61ee8fb301712f56ff4bbf2 Mon Sep 17 00:00:00 2001 From: darkpotatoo <63860602+darkpotatoo@users.noreply.github.com> Date: Wed, 31 Dec 2025 13:54:16 -0500 Subject: [PATCH 6/7] requested changes --- .../frc/robot/generated/TunerConstants.java | 2 +- .../drive/PhoenixOdometryThread.java | 20 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 5da546d..1eb1f4d 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -24,7 +24,7 @@ // Generated using https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. + // Both sets of gains need to be tuned to yoSwur individual robot. // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 7f908c9..7fce228 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -58,7 +58,7 @@ public synchronized void start() { public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); - SwerveDriveReal.ODOMETRY_LOCK.lock(); + SwerveDriveIO.ODOMETRY_LOCK.lock(); try { BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); @@ -67,7 +67,7 @@ public Queue registerSignal(StatusSignal signal) { phoenixQueues.add(queue); } finally { signalsLock.unlock(); - SwerveDriveReal.ODOMETRY_LOCK.unlock(); + SwerveDriveIO.ODOMETRY_LOCK.unlock(); } return queue; } @@ -76,13 +76,13 @@ public Queue registerSignal(StatusSignal signal) { public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); - SwerveDriveReal.ODOMETRY_LOCK.lock(); + SwerveDriveIO.ODOMETRY_LOCK.lock(); try { genericSignals.add(signal); genericQueues.add(queue); } finally { signalsLock.unlock(); - SwerveDriveReal.ODOMETRY_LOCK.unlock(); + SwerveDriveIO.ODOMETRY_LOCK.unlock(); } return queue; } @@ -90,11 +90,11 @@ public Queue registerSignal(DoubleSupplier signal) { /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); - SwerveDriveReal.ODOMETRY_LOCK.lock(); + SwerveDriveIO.ODOMETRY_LOCK.lock(); try { timestampQueues.add(queue); } finally { - SwerveDriveReal.ODOMETRY_LOCK.unlock(); + SwerveDriveIO.ODOMETRY_LOCK.unlock(); } return queue; } @@ -106,12 +106,12 @@ public void run() { signalsLock.lock(); try { if (IS_CANFD && phoenixSignals.length > 0) { - BaseStatusSignal.waitForAll(2.0 / SwerveDriveReal.ODOMETRY_FREQUENCY, phoenixSignals); + BaseStatusSignal.waitForAll(2.0 / SwerveDriveIO.ODOMETRY_FREQUENCY, phoenixSignals); } else { // "waitForAll" does not support blocking on multiple signals with a bus // that is not CAN FD, regardless of Pro licensing. No reasoning for this // behavior is provided by the documentation. - Thread.sleep((long) (1000.0 / SwerveDriveReal.ODOMETRY_FREQUENCY)); + Thread.sleep((long) (1000.0 / SwerveDriveIO.ODOMETRY_FREQUENCY)); if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); } } catch (InterruptedException e) { @@ -122,7 +122,7 @@ public void run() { } // Save new data to queues - SwerveDriveReal.ODOMETRY_LOCK.lock(); + SwerveDriveIO.ODOMETRY_LOCK.lock(); try { // Sample timestamp is current FPGA time minus average CAN latency // Default timestamps from Phoenix are NOT compatible with @@ -147,7 +147,7 @@ public void run() { timestampQueue.offer(timestamp); } } finally { - SwerveDriveReal.ODOMETRY_LOCK.unlock(); + SwerveDriveIO.ODOMETRY_LOCK.unlock(); } } } From 9374a4f11b8b586ec2785ddfb10025598ec6b6ae Mon Sep 17 00:00:00 2001 From: darkpotatoo <63860602+darkpotatoo@users.noreply.github.com> Date: Wed, 31 Dec 2025 17:21:44 -0500 Subject: [PATCH 7/7] how did i even do that --- src/main/java/frc/robot/generated/TunerConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 1eb1f4d..5da546d 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -24,7 +24,7 @@ // Generated using https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to yoSwur individual robot. + // Both sets of gains need to be tuned to your individual robot. // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus