From 049eab6c2180b5a5f435d38a382eadd15e76a716 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sun, 27 Sep 2026 12:13:22 -0400 Subject: [PATCH 01/24] Swerve clamping test (see output at https://www.desmos.com/calculator/jhab2wuhrr) --- src/main/java/test/SwerveClampingTest.java | 40 ++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 src/main/java/test/SwerveClampingTest.java diff --git a/src/main/java/test/SwerveClampingTest.java b/src/main/java/test/SwerveClampingTest.java new file mode 100644 index 00000000..cdfb3d5b --- /dev/null +++ b/src/main/java/test/SwerveClampingTest.java @@ -0,0 +1,40 @@ +package test; + +import java.io.File; +import java.io.IOException; +import java.io.PrintWriter; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.subsystems.drive.DriveSubsystem; + +public class SwerveClampingTest { + public static void run(int iterations, double dreamLevel) { + try { + File log = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test.csv"); + PrintWriter writer = new PrintWriter(log); + + ChassisSpeeds speeds = new ChassisSpeeds(); + ChassisSpeeds speedsClamped; + Translation2d centerOfRotation = new Translation2d(); + for (int i = 0; i < iterations; i++) { + boolean driveMax = Math.random() > 0.5; + if (driveMax) { + speeds.vxMetersPerSecond = 1; + speeds.omegaRadiansPerSecond = Math.random(); + } else { + speeds.vxMetersPerSecond = Math.random(); + speeds.omegaRadiansPerSecond = 1; + } + speedsClamped = DriveSubsystem.inceptionMode(speeds, centerOfRotation, dreamLevel); + writer.println(speeds.vxMetersPerSecond + ", " + speeds.omegaRadiansPerSecond + + ", " + speedsClamped.vxMetersPerSecond + ", " + speedsClamped.omegaRadiansPerSecond); + } + + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + }; + } +} From 5cd4fcc7d37609f500afc3cae81be341f3dee3f8 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sun, 27 Sep 2026 13:07:07 -0400 Subject: [PATCH 02/24] Attempt at full-stack test --- src/main/java/test/SwerveClampingTest.java | 72 +++++++++++++++++++++- 1 file changed, 70 insertions(+), 2 deletions(-) diff --git a/src/main/java/test/SwerveClampingTest.java b/src/main/java/test/SwerveClampingTest.java index cdfb3d5b..07ab95ed 100644 --- a/src/main/java/test/SwerveClampingTest.java +++ b/src/main/java/test/SwerveClampingTest.java @@ -7,10 +7,14 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.drive.DriveWeightCommand; +import frc.robot.subsystems.drive.weights.JoystickDriveWeight; public class SwerveClampingTest { - public static void run(int iterations, double dreamLevel) { + + public static void run(int samples, double dreamLevel) { try { File log = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test.csv"); PrintWriter writer = new PrintWriter(log); @@ -18,7 +22,7 @@ public static void run(int iterations, double dreamLevel) { ChassisSpeeds speeds = new ChassisSpeeds(); ChassisSpeeds speedsClamped; Translation2d centerOfRotation = new Translation2d(); - for (int i = 0; i < iterations; i++) { + for (int i = 0; i < samples; i++) { boolean driveMax = Math.random() > 0.5; if (driveMax) { speeds.vxMetersPerSecond = 1; @@ -37,4 +41,68 @@ public static void run(int iterations, double dreamLevel) { e.printStackTrace(); }; } + + public static void runFull(int samples, double dreamLevel, boolean slowMode, boolean fastMode) { + try { + File log = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test.csv"); + PrintWriter writer = new PrintWriter(log); + + JoystickInputContainer input = new JoystickInputContainer(); + ChassisSpeeds speedsClamped; + Translation2d centerOfRotation = new Translation2d(); + + JoystickDriveWeight joystickDriveWeight = new JoystickDriveWeight( + () -> input.getXPercent(), () -> 0.0, () -> input.getOmegaPercent(), + () -> slowMode, () -> fastMode, + () -> true, null, () -> false); + + DriveWeightCommand.addPersistentWeight(joystickDriveWeight); + + for (int i = 0; i < samples; i++) { + boolean driveMax = Math.random() > 0.5; + if (driveMax) { + input.setXPercent(1.0); + input.setOmegaPercent(Math.random()); + } else { + input.setXPercent(Math.random()); + input.setOmegaPercent(1.0); + } + + ChassisSpeeds speedsPercent = DriveWeightCommand.getAllSpeeds(); + speedsPercent.vxMetersPerSecond /= DriveConstants.maxSpeed; + speedsPercent.vyMetersPerSecond /= DriveConstants.maxSpeed; + speedsPercent.omegaRadiansPerSecond /= DriveConstants.maxOmega; + + speedsClamped = DriveSubsystem.inceptionMode(speedsPercent, centerOfRotation, dreamLevel); + + writer.println(input.getXPercent() + ", " + input.getOmegaPercent() + + ", " + speedsClamped.vxMetersPerSecond + ", " + speedsClamped.omegaRadiansPerSecond); + } + + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + }; + } + + private static class JoystickInputContainer { + private double xPercent = 0.0; + private double omegaPercent = 0.0; + + public void setXPercent(double xPercent) { + this.xPercent = xPercent; + } + + public void setOmegaPercent(double omegaPercent) { + this.omegaPercent = omegaPercent; + } + + public double getXPercent() { + return xPercent; + } + + public double getOmegaPercent() { + return omegaPercent; + } + } } From a6719bfbb099102593f891568bee411e94485d8e Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sun, 27 Sep 2026 15:54:47 -0400 Subject: [PATCH 03/24] Commented bad method --- src/main/java/test/SwerveClampingTest.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/test/SwerveClampingTest.java b/src/main/java/test/SwerveClampingTest.java index 07ab95ed..455a61b3 100644 --- a/src/main/java/test/SwerveClampingTest.java +++ b/src/main/java/test/SwerveClampingTest.java @@ -42,7 +42,7 @@ public static void run(int samples, double dreamLevel) { }; } - public static void runFull(int samples, double dreamLevel, boolean slowMode, boolean fastMode) { + /* public static void runFull(int samples, double dreamLevel, boolean slowMode, boolean fastMode) { try { File log = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test.csv"); PrintWriter writer = new PrintWriter(log); @@ -83,7 +83,7 @@ public static void runFull(int samples, double dreamLevel, boolean slowMode, boo } catch (IOException e) { e.printStackTrace(); }; - } + } */ private static class JoystickInputContainer { private double xPercent = 0.0; From 3be3317a559b4338656be72486ca82e74817c85b Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 30 Sep 2026 17:38:59 -0400 Subject: [PATCH 04/24] Logger attempt that will not work --- src/main/java/test/SwerveClampingTest.java | 92 +++++++++++++++++++++- 1 file changed, 89 insertions(+), 3 deletions(-) diff --git a/src/main/java/test/SwerveClampingTest.java b/src/main/java/test/SwerveClampingTest.java index 455a61b3..b2d57a4c 100644 --- a/src/main/java/test/SwerveClampingTest.java +++ b/src/main/java/test/SwerveClampingTest.java @@ -1,22 +1,27 @@ package test; +import java.io.Closeable; import java.io.File; import java.io.IOException; import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.List; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.subsystems.drive.DriveConstants; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.DriveSubsystem; -import frc.robot.subsystems.drive.DriveWeightCommand; -import frc.robot.subsystems.drive.weights.JoystickDriveWeight; public class SwerveClampingTest { public static void run(int samples, double dreamLevel) { try { File log = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test.csv"); + log.createNewFile(); PrintWriter writer = new PrintWriter(log); ChassisSpeeds speeds = new ChassisSpeeds(); @@ -105,4 +110,85 @@ public double getOmegaPercent() { return omegaPercent; } } + + public static class SwerveTestInputRemapper extends CommandXboxController { + public SwerveTestInputRemapper(int port) { + super(port); + } + + @Override + public double getLeftX() { + return MathUtil.applyDeadband(Math.hypot(super.getLeftX(), super.getLeftY()), 0.03); + } + + @Override + public double getLeftY() { + return 0; + } + + @Override + public double getRightX() { + return MathUtil.applyDeadband(Math.hypot(super.getRightX(), super.getRightY()), 0.03); + } + + @Override + public double getRightY() { + return 0; + } + } + + public static class ChassisSpeedsLogger implements Closeable { + private File logFile; + private PrintWriter writer; + + private TimeInterpolatableBuffer input; + private TimeInterpolatableBuffer output; + private double lastTimeSeconds = 0; + + public ChassisSpeedsLogger() { + input = createChassisSpeedsBuffer(1); + output = createChassisSpeedsBuffer(1); + try { + logFile = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test__input.csv"); + logFile.createNewFile(); + writer = new PrintWriter(logFile); + } catch (IOException e) { + e.printStackTrace(); + } + } + + private static TimeInterpolatableBuffer createChassisSpeedsBuffer(int historySizeSeconds) { + return TimeInterpolatableBuffer.createBuffer( + (x1, x2, t) -> new ChassisSpeeds( + x1.vxMetersPerSecond + t * (x2.vxMetersPerSecond - x1.vxMetersPerSecond), + x1.vyMetersPerSecond + t * (x2.vyMetersPerSecond - x1.vyMetersPerSecond), + x1.omegaRadiansPerSecond + t * (x2.omegaRadiansPerSecond - x1.omegaRadiansPerSecond)), + historySizeSeconds); + } + + public void addInputMeasurement(double timeSeconds, ChassisSpeeds speeds) { + input.addSample(timeSeconds, speeds); + if (timeSeconds > lastTimeSeconds) lastTimeSeconds = timeSeconds; + } + + public void addOutputMeasurement(double timeSeconds, ChassisSpeeds speeds) { + output.addSample(timeSeconds, speeds); + if (timeSeconds > lastTimeSeconds) lastTimeSeconds = timeSeconds; + } + + public void log() { + ChassisSpeeds inputSpeeds = input.getSample(lastTimeSeconds).orElse(null); + ChassisSpeeds outputSpeeds = output.getSample(lastTimeSeconds).orElse(null); + if (inputSpeeds == null || outputSpeeds == null) { + return; + } + writer.println(inputSpeeds.vxMetersPerSecond + ", " + inputSpeeds.omegaRadiansPerSecond + + ", " + outputSpeeds.vxMetersPerSecond + ", " + outputSpeeds.omegaRadiansPerSecond); + } + + @Override + public void close() { + writer.close(); + } + } } From 6f760e2344680aab19bd2cc109ff9dc0c108f397 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sat, 3 Oct 2026 15:42:39 -0400 Subject: [PATCH 05/24] Start --- src/main/java/frc/robot/RobotCommands.java | 9 +++++++++ src/main/java/frc/robot/RobotContainer.java | 11 +++++++---- src/main/java/frc/robot/constants/RobotConstants.java | 10 +++++----- .../frc/robot/subsystems/drive/DriveSubsystem.java | 3 ++- 4 files changed, 23 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index 6a527126..59df153c 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -15,6 +15,7 @@ import frc.robot.sensors.gyro.BumpDetectorPeriodic; import frc.robot.sensors.odometry.RobotOdometry; import frc.robot.subsystems.ShotControl; +import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.hood.HoodSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; @@ -206,4 +207,12 @@ public Command clearBumpCommand(Pose2d endPose, double velocity) { public Command autoDelayCommand() { return Commands.deferredProxy(() -> new WaitCommand(SmartDashboard.getNumber("AutoWaitTime", 0.0))); } + + public Command testDriveCommand() { + return driveSubsystem.runVelocityCommand(() -> new ChassisSpeeds(DriveConstants.maxSpeed, 0, 0), () -> false); + } + + public Command testDriveAndRotateCommand() { + return driveSubsystem.runVelocityCommand(() -> new ChassisSpeeds(DriveConstants.maxSpeed, 0, DriveConstants.maxOmega), () -> false); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7075517..abb4dcec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -208,6 +208,9 @@ private void configureBindings() { | DRIVE CONTROLLER | ------------------*/ + driveController.x().whileTrue(robotCommands.testDriveCommand()); + driveController.y().whileTrue(robotCommands.testDriveAndRotateCommand()); + driveController.back().onTrue(new InstantCommand(() -> ShotControl.getInstance().toggleOffsetHubShot())); driveController.start().onTrue(RobotOdometry.instance.resetGyroCommand(() -> new Rotation2d())); // DriveWeightCommand.createWeightTrigger(driveToPointWeight, () -> @@ -226,10 +229,10 @@ private void configureBindings() { driveController.rightTrigger().whileTrue(shootCommand()).onFalse(robotCommands.finishShootCommand()); - driveController.y() - .toggleOnTrue(intakeSubsystem.intakeUpCommand() - .until(() -> intakeSubsystem.isAtPosition(IntakeConstants.stowedPositionRadians)) - .andThen(intakeSubsystem.intakeHoldCommand(IntakeConstants.stowedPositionRadians))); + // driveController.y() + // .toggleOnTrue(intakeSubsystem.intakeUpCommand() + // .until(() -> intakeSubsystem.isAtPosition(IntakeConstants.stowedPositionRadians)) + // .andThen(intakeSubsystem.intakeHoldCommand(IntakeConstants.stowedPositionRadians))); driveController.pov(0).whileTrue(shootCommand().beforeStarting(() -> { ShotControl.getInstance().setManualSetpoint(ShotControl.towerManualSetpoint); diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 8feba698..fc769850 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -19,7 +19,7 @@ public class RobotConstants { public class RobotInformation { - public static final RobotType robot = RobotTypes.prime26; + public static final RobotType robot = RobotTypes.deux26; } public class RobotTypes { @@ -41,10 +41,10 @@ public class RobotTypes { intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) .addAprilTagCamera(CameraConstants.primeRightCamera) .addAprilTagCamera(CameraConstants.primeBackCamera); - public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem, kickerSubsystem, - spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, shooterSubsystem, - hoodSubsystem).addAprilTagCamera(CameraConstants.duexLeftCamera) - .addAprilTagCamera(CameraConstants.deuxBackCamera); + public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem, + // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, shooterSubsystem, + // hoodSubsystem).addAprilTagCamera(CameraConstants.duexLeftCamera) + // .addAprilTagCamera(CameraConstants.deuxBackCamera); public static final RobotType frank25 = new RobotType("Frank25", driveSubsystem); public static final RobotType prime25 = new RobotType("Prime25", driveSubsystem); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 6b437179..6703661b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -222,6 +222,7 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream Logger.recordOutput("Drive/SwerveStates/Input", DriveConstants.kinematics.toSwerveModuleStates(speeds)); Logger.recordOutput("Drive/SwerveStates/DoubleCone", DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)); + Logger.recordOutput("Drive/SwerveStates/InputStates", DriveConstants.kinematics.toSwerveModuleStates(speeds)); // speedsOptimized = ChassisSpeeds.discretize(speedsOptimized, 0.02); for (int i = 0; i < 4; i++) { modules[i].setDesiredStateMetersPerSecond(previousSetpoint.moduleStates()[i]); @@ -289,7 +290,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { - return new RunCommand(() -> runVelocity(speeds.get(), true, 3, limitSpeeds), this).finallyDo(() -> stop()); + return new RunCommand(() -> runVelocity(speeds.get(), true, 1, limitSpeeds), this).finallyDo(() -> stop()); } public Consumer runVelocityConsumer() { From 5ce813cf46066ed25c41b10ab9356ceb1f550c06 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 7 Oct 2026 19:45:06 -0400 Subject: [PATCH 06/24] Swerve steer sysid routine --- .../subsystems/drive/DriveSubsystem.java | 38 +++++++++++++++---- .../util/sysid/SwerveDriveSysidRoutine.java | 12 +++++- .../frc/robot/util/sysid/SysIdChooser.java | 11 ++++-- 3 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 6703661b..3073a00f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -56,7 +56,8 @@ public class DriveSubsystem extends SubsystemPlatform { private final Module[] modules = new Module[4]; // FL, FR, BL, BR public RobotConfig config; public Gyro gyro; - public SysIdRoutine sysIdRoutine; + public SysIdRoutine driveSysIdRoutine; + public SysIdRoutine steerSysIdRoutine; private final SwerveSetpointGenerator setpointGenerator; private SwerveSetpoint previousSetpoint; public static final Lock odometryLock = new ReentrantLock(); @@ -77,9 +78,22 @@ public DriveSubsystem(Gyro gyro) { modules[3] = new Module(DriveSubsystem.getIOByMode(DriveConstants.BR), PivotId.BR); // custom format - sysIdRoutine = - new SwerveDriveSysidRoutine() - .createNewRoutine( + driveSysIdRoutine = + SwerveDriveSysidRoutine + .createDriveRoutine( + modules[0], + modules[1], + modules[2], + modules[3], + this, + new SysIdRoutine.Config( + Volts.per(Seconds).of(2), + Volts.of(7), + Seconds.of(5), + (state) -> Logger.recordOutput("SysIdTestState", state.toString()))); + steerSysIdRoutine = + SwerveDriveSysidRoutine + .createSteerRoutine( modules[0], modules[1], modules[2], @@ -281,12 +295,20 @@ public static double computeMaxNorm(Translation2d[] translations, Translation2d .orElseThrow(() -> new NoSuchElementException("No max norm.")); } - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysIdRoutine.quasistatic(direction); + public Command sysIdQuasistaticDrive(SysIdRoutine.Direction direction) { + return driveSysIdRoutine.quasistatic(direction); + } + + public Command sysIdDynamicDrive(SysIdRoutine.Direction direction) { + return driveSysIdRoutine.dynamic(direction); + } + + public Command sysIdQuasistaticSteer(SysIdRoutine.Direction direction) { + return steerSysIdRoutine.quasistatic(direction); } - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysIdRoutine.dynamic(direction); + public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { + return steerSysIdRoutine.dynamic(direction); } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { diff --git a/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java b/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java index 487539e8..18d0a1cc 100644 --- a/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java +++ b/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java @@ -25,7 +25,7 @@ public class SwerveDriveSysidRoutine { * @param config * config for sysid */ - public SysIdRoutine createNewRoutine(Module fl, Module fr, Module bl, Module br, SubsystemBase subsystem, + public static SysIdRoutine createDriveRoutine(Module fl, Module fr, Module bl, Module br, SubsystemBase subsystem, SysIdRoutine.Config config) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism((Voltage volts) -> { fl.setDriveVoltage(-volts.in(Volts)); @@ -34,4 +34,14 @@ public SysIdRoutine createNewRoutine(Module fl, Module fr, Module bl, Module br, br.setDriveVoltage(-volts.in(Volts)); }, null, subsystem)); } + + public static SysIdRoutine createSteerRoutine(Module fl, Module fr, Module bl, Module br, SubsystemBase subsystem, + SysIdRoutine.Config config) { + return new SysIdRoutine(config, new SysIdRoutine.Mechanism((Voltage voltage) -> { + fl.setSteerVoltage(voltage.in(Volts)); + fr.setSteerVoltage(voltage.in(Volts)); + bl.setSteerVoltage(voltage.in(Volts)); + br.setSteerVoltage(voltage.in(Volts)); + }, null, subsystem)); + } } diff --git a/src/main/java/frc/robot/util/sysid/SysIdChooser.java b/src/main/java/frc/robot/util/sysid/SysIdChooser.java index daed6580..67ced96f 100644 --- a/src/main/java/frc/robot/util/sysid/SysIdChooser.java +++ b/src/main/java/frc/robot/util/sysid/SysIdChooser.java @@ -37,9 +37,14 @@ public void sysIdInit() { BooleanSupplier startNext = controller.a(); BooleanSupplier cancel = controller.b(); - sysIdChooser.addOption("Swerve SysId", - CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistatic, driveSubsystem::sysIdDynamic, - "Swerve", startNext, cancel, + sysIdChooser.addOption("Swerve Drive SysId", + CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistaticDrive, driveSubsystem::sysIdDynamicDrive, + "SwerveDrive", startNext, cancel, + () -> CommandScheduler.getInstance().schedule(driveSubsystem.stopCommand()))); + + sysIdChooser.addOption("Swerve Steer SysId", + CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistaticSteer, driveSubsystem::sysIdDynamicSteer, + "SwerveSteer", startNext, cancel, () -> CommandScheduler.getInstance().schedule(driveSubsystem.stopCommand()))); sysIdChooser.addOption("Turret SysId", From c647daba5d43c26bb3b424e519a4d5aaee9ea9a3 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 7 Oct 2026 22:25:09 -0400 Subject: [PATCH 07/24] Sysid + spotless --- src/main/java/frc/robot/RobotCommands.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 24 ++-- .../frc/robot/constants/RobotConstants.java | 7 +- .../robot/constants/RobotPIDConstants.java | 2 + .../frc/robot/util/sysid/SysIdChooser.java | 10 +- src/main/java/test/SwerveClampingTest.java | 110 ++++++++---------- 6 files changed, 75 insertions(+), 81 deletions(-) diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java index 3683b393..9a82b4b3 100644 --- a/src/main/java/frc/robot/RobotCommands.java +++ b/src/main/java/frc/robot/RobotCommands.java @@ -220,6 +220,7 @@ public Command testDriveCommand() { } public Command testDriveAndRotateCommand() { - return driveSubsystem.runVelocityCommand(() -> new ChassisSpeeds(DriveConstants.maxSpeed, 0, DriveConstants.maxOmega), () -> false); + return driveSubsystem.runVelocityCommand( + () -> new ChassisSpeeds(DriveConstants.maxSpeed, 0, DriveConstants.maxOmega), () -> false); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 471422ff..758ca499 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,13 +42,11 @@ import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.drive.DriveWeightCommand; -import frc.robot.subsystems.drive.weights.AutoAlignWeight; import frc.robot.subsystems.drive.weights.DriveToPoint; import frc.robot.subsystems.drive.weights.JoystickDriveWeight; import frc.robot.subsystems.drive.weights.LockToPointWeight; import frc.robot.subsystems.drive.weights.ShotCorrectionWeight; import frc.robot.subsystems.hood.HoodSubsystem; -import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.intakeRollers.IntakeRollerSubsystem; import frc.robot.subsystems.kicker.KickerSubsystem; @@ -56,8 +54,6 @@ import frc.robot.subsystems.spindexer.SpindexerSubsystem; import frc.robot.subsystems.turret.TurretConstants; import frc.robot.subsystems.turret.TurretSubsystem; -import frc.robot.util.autoalign.system.controller.impl.AutopilotAlignController; -import frc.robot.util.autoalign.system.pointprovider.impl.SinglePointProvider; import frc.robot.util.autonomous.AutonBuilder; import frc.robot.util.autonomous.AutonChooser; import frc.robot.util.helpers.AllianceManager; @@ -101,7 +97,7 @@ public class RobotContainer { private DriveToPoint driveToPointWeight; private ShotCorrectionWeight shotCorrectionWeight; private LockToPointWeight lockToPointWeight; - private AutoAlignWeight driveAlignSysWeight; + // private AutoAlignWeight driveAlignSysWeight; // dashboards private SysIdChooser sysIdChooser; @@ -157,10 +153,11 @@ public RobotContainer() { () -> (!RobotState.isTest() ? driveController : pitController).a().getAsBoolean(), 4); DriveWeightCommand.addPersistentWeight(joystickDriveWeight); - driveAlignSysWeight = new AutoAlignWeight( - new AutopilotAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, Rotation2d.k180deg), - () -> RobotOdometry.instance.getPose("Main"))), - driveSubsystem); + // driveAlignSysWeight = new AutoAlignWeight( + // new AutopilotAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, + // Rotation2d.k180deg), + // () -> RobotOdometry.instance.getPose("Main"))), + // driveSubsystem); driveToPointWeight = new DriveToPoint(() -> RobotOdometry.instance.getPose("Main"), () -> new Pose2d( AllianceManager.chooseFromAlliance(FieldConstants.blueTowerBarCenter, FieldConstants.redTowerBarCenter), @@ -187,7 +184,7 @@ public RobotContainer() { RobotOdometry.instance.addGyroOffset(AllianceManager.chooseFromAlliance(Rotation2d.kZero, Rotation2d.kPi)); }); autonChooser = new AutonChooser(); - sysIdChooser = new SysIdChooser(driveSubsystem, shooterSubsystem, turretSubsystem, pitController); + sysIdChooser = new SysIdChooser(driveSubsystem, shooterSubsystem, turretSubsystem, testController); projectileLogger = new ProjectileLogger(robotCommands); periodicLogging = new PeriodicLogging(); @@ -239,9 +236,10 @@ private void configureBindings() { driveController.rightTrigger().whileTrue(shootCommand()).onFalse(robotCommands.finishShootCommand()); // driveController.y() - // .toggleOnTrue(intakeSubsystem.intakeUpCommand() - // .until(() -> intakeSubsystem.isAtPosition(IntakeConstants.stowedPositionRadians)) - // .andThen(intakeSubsystem.intakeHoldCommand(IntakeConstants.stowedPositionRadians))); + // .toggleOnTrue(intakeSubsystem.intakeUpCommand() + // .until(() -> + // intakeSubsystem.isAtPosition(IntakeConstants.stowedPositionRadians)) + // .andThen(intakeSubsystem.intakeHoldCommand(IntakeConstants.stowedPositionRadians))); driveController.pov(0).whileTrue(shootCommand().beforeStarting(() -> { ShotControl.getInstance().setManualSetpoint(ShotControl.towerManualSetpoint); diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 7be4dbdd..8cc393d9 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -50,9 +50,10 @@ public class RobotTypes { .addAprilTagCamera(CameraConstants.primeRightCamera) .addAprilTagCamera(CameraConstants.primeBackCamera); public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem, - // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, shooterSubsystem, - // hoodSubsystem).addAprilTagCamera(CameraConstants.duexLeftCamera) - // .addAprilTagCamera(CameraConstants.deuxBackCamera); + // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, + // shooterSubsystem, + // hoodSubsystem).addAprilTagCamera(CameraConstants.duexLeftCamera) + // .addAprilTagCamera(CameraConstants.deuxBackCamera); public static final RobotType frank25 = new RobotType("Frank25", driveSubsystem); public static final RobotType prime25 = new RobotType("Prime25", driveSubsystem); diff --git a/src/main/java/frc/robot/constants/RobotPIDConstants.java b/src/main/java/frc/robot/constants/RobotPIDConstants.java index 131709bf..5014da33 100644 --- a/src/main/java/frc/robot/constants/RobotPIDConstants.java +++ b/src/main/java/frc/robot/constants/RobotPIDConstants.java @@ -24,6 +24,8 @@ public class RobotPIDConstants { public static final PIDConstants drivePid = new PIDConstants(0.17189, 0.0, 0); public static final FeedForwardConstants driveFF = new FeedForwardConstants(0.12506, 2, 0.27879); public static final PIDConstants steerPid = new PIDConstants(0.725, 0.0, 0.005); + // TODO compare results across modules, run one by one + public static final FeedForwardConstants steerFF = new FeedForwardConstants(0.23978, 0.4246, 0.062529); public static final PIDConstants intakeAnglePidReal = Switchable.of(new PIDConstants(4.5, 0, 0)) .addAlt(RobotTypes.prime26, new PIDConstants(2.6, 0, 0)).get(); public static final FeedForwardConstants intakeFFReal = Switchable.of(new FeedForwardConstants(0, 0, 0)).get(); diff --git a/src/main/java/frc/robot/util/sysid/SysIdChooser.java b/src/main/java/frc/robot/util/sysid/SysIdChooser.java index 67ced96f..78d210e5 100644 --- a/src/main/java/frc/robot/util/sysid/SysIdChooser.java +++ b/src/main/java/frc/robot/util/sysid/SysIdChooser.java @@ -38,13 +38,13 @@ public void sysIdInit() { BooleanSupplier cancel = controller.b(); sysIdChooser.addOption("Swerve Drive SysId", - CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistaticDrive, driveSubsystem::sysIdDynamicDrive, - "SwerveDrive", startNext, cancel, + CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistaticDrive, + driveSubsystem::sysIdDynamicDrive, "SwerveDrive", startNext, cancel, () -> CommandScheduler.getInstance().schedule(driveSubsystem.stopCommand()))); - + sysIdChooser.addOption("Swerve Steer SysId", - CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistaticSteer, driveSubsystem::sysIdDynamicSteer, - "SwerveSteer", startNext, cancel, + CreateSysIdCommand.createCommand(driveSubsystem::sysIdQuasistaticSteer, + driveSubsystem::sysIdDynamicSteer, "SwerveSteer", startNext, cancel, () -> CommandScheduler.getInstance().schedule(driveSubsystem.stopCommand()))); sysIdChooser.addOption("Turret SysId", diff --git a/src/main/java/test/SwerveClampingTest.java b/src/main/java/test/SwerveClampingTest.java index b2d57a4c..80e88bcf 100644 --- a/src/main/java/test/SwerveClampingTest.java +++ b/src/main/java/test/SwerveClampingTest.java @@ -4,11 +4,8 @@ import java.io.File; import java.io.IOException; import java.io.PrintWriter; -import java.util.ArrayList; -import java.util.List; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -37,58 +34,49 @@ public static void run(int samples, double dreamLevel) { speeds.omegaRadiansPerSecond = 1; } speedsClamped = DriveSubsystem.inceptionMode(speeds, centerOfRotation, dreamLevel); - writer.println(speeds.vxMetersPerSecond + ", " + speeds.omegaRadiansPerSecond - + ", " + speedsClamped.vxMetersPerSecond + ", " + speedsClamped.omegaRadiansPerSecond); + writer.println(speeds.vxMetersPerSecond + ", " + speeds.omegaRadiansPerSecond + ", " + + speedsClamped.vxMetersPerSecond + ", " + speedsClamped.omegaRadiansPerSecond); } - + writer.close(); } catch (IOException e) { e.printStackTrace(); - }; + } ; } - /* public static void runFull(int samples, double dreamLevel, boolean slowMode, boolean fastMode) { - try { - File log = new File(Filesystem.getDeployDirectory() + "/test/swerve_clamping_test.csv"); - PrintWriter writer = new PrintWriter(log); - - JoystickInputContainer input = new JoystickInputContainer(); - ChassisSpeeds speedsClamped; - Translation2d centerOfRotation = new Translation2d(); - - JoystickDriveWeight joystickDriveWeight = new JoystickDriveWeight( - () -> input.getXPercent(), () -> 0.0, () -> input.getOmegaPercent(), - () -> slowMode, () -> fastMode, - () -> true, null, () -> false); - - DriveWeightCommand.addPersistentWeight(joystickDriveWeight); - - for (int i = 0; i < samples; i++) { - boolean driveMax = Math.random() > 0.5; - if (driveMax) { - input.setXPercent(1.0); - input.setOmegaPercent(Math.random()); - } else { - input.setXPercent(Math.random()); - input.setOmegaPercent(1.0); - } - - ChassisSpeeds speedsPercent = DriveWeightCommand.getAllSpeeds(); - speedsPercent.vxMetersPerSecond /= DriveConstants.maxSpeed; - speedsPercent.vyMetersPerSecond /= DriveConstants.maxSpeed; - speedsPercent.omegaRadiansPerSecond /= DriveConstants.maxOmega; - - speedsClamped = DriveSubsystem.inceptionMode(speedsPercent, centerOfRotation, dreamLevel); - - writer.println(input.getXPercent() + ", " + input.getOmegaPercent() - + ", " + speedsClamped.vxMetersPerSecond + ", " + speedsClamped.omegaRadiansPerSecond); - } - - writer.close(); - } catch (IOException e) { - e.printStackTrace(); - }; - } */ + /* + * public static void runFull(int samples, double dreamLevel, boolean slowMode, + * boolean fastMode) { try { File log = new File(Filesystem.getDeployDirectory() + * + "/test/swerve_clamping_test.csv"); PrintWriter writer = new + * PrintWriter(log); + * + * JoystickInputContainer input = new JoystickInputContainer(); ChassisSpeeds + * speedsClamped; Translation2d centerOfRotation = new Translation2d(); + * + * JoystickDriveWeight joystickDriveWeight = new JoystickDriveWeight( () -> + * input.getXPercent(), () -> 0.0, () -> input.getOmegaPercent(), () -> + * slowMode, () -> fastMode, () -> true, null, () -> false); + * + * DriveWeightCommand.addPersistentWeight(joystickDriveWeight); + * + * for (int i = 0; i < samples; i++) { boolean driveMax = Math.random() > 0.5; + * if (driveMax) { input.setXPercent(1.0); input.setOmegaPercent(Math.random()); + * } else { input.setXPercent(Math.random()); input.setOmegaPercent(1.0); } + * + * ChassisSpeeds speedsPercent = DriveWeightCommand.getAllSpeeds(); + * speedsPercent.vxMetersPerSecond /= DriveConstants.maxSpeed; + * speedsPercent.vyMetersPerSecond /= DriveConstants.maxSpeed; + * speedsPercent.omegaRadiansPerSecond /= DriveConstants.maxOmega; + * + * speedsClamped = DriveSubsystem.inceptionMode(speedsPercent, centerOfRotation, + * dreamLevel); + * + * writer.println(input.getXPercent() + ", " + input.getOmegaPercent() + ", " + + * speedsClamped.vxMetersPerSecond + ", " + + * speedsClamped.omegaRadiansPerSecond); } + * + * writer.close(); } catch (IOException e) { e.printStackTrace(); }; } + */ private static class JoystickInputContainer { private double xPercent = 0.0; @@ -158,22 +146,26 @@ public ChassisSpeedsLogger() { } private static TimeInterpolatableBuffer createChassisSpeedsBuffer(int historySizeSeconds) { - return TimeInterpolatableBuffer.createBuffer( - (x1, x2, t) -> new ChassisSpeeds( - x1.vxMetersPerSecond + t * (x2.vxMetersPerSecond - x1.vxMetersPerSecond), - x1.vyMetersPerSecond + t * (x2.vyMetersPerSecond - x1.vyMetersPerSecond), - x1.omegaRadiansPerSecond + t * (x2.omegaRadiansPerSecond - x1.omegaRadiansPerSecond)), - historySizeSeconds); + return TimeInterpolatableBuffer + .createBuffer( + (x1, x2, t) -> new ChassisSpeeds( + x1.vxMetersPerSecond + t * (x2.vxMetersPerSecond - x1.vxMetersPerSecond), + x1.vyMetersPerSecond + t * (x2.vyMetersPerSecond - x1.vyMetersPerSecond), + x1.omegaRadiansPerSecond + + t * (x2.omegaRadiansPerSecond - x1.omegaRadiansPerSecond)), + historySizeSeconds); } public void addInputMeasurement(double timeSeconds, ChassisSpeeds speeds) { input.addSample(timeSeconds, speeds); - if (timeSeconds > lastTimeSeconds) lastTimeSeconds = timeSeconds; + if (timeSeconds > lastTimeSeconds) + lastTimeSeconds = timeSeconds; } public void addOutputMeasurement(double timeSeconds, ChassisSpeeds speeds) { output.addSample(timeSeconds, speeds); - if (timeSeconds > lastTimeSeconds) lastTimeSeconds = timeSeconds; + if (timeSeconds > lastTimeSeconds) + lastTimeSeconds = timeSeconds; } public void log() { @@ -182,8 +174,8 @@ public void log() { if (inputSpeeds == null || outputSpeeds == null) { return; } - writer.println(inputSpeeds.vxMetersPerSecond + ", " + inputSpeeds.omegaRadiansPerSecond - + ", " + outputSpeeds.vxMetersPerSecond + ", " + outputSpeeds.omegaRadiansPerSecond); + writer.println(inputSpeeds.vxMetersPerSecond + ", " + inputSpeeds.omegaRadiansPerSecond + ", " + + outputSpeeds.vxMetersPerSecond + ", " + outputSpeeds.omegaRadiansPerSecond); } @Override From 7204826889758b14763dd0a3ace8b9ff2b41df0e Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 7 Oct 2026 22:25:28 -0400 Subject: [PATCH 08/24] Steer velocity setpoint --- .../subsystems/drive/DriveConstants.java | 2 + .../frc/robot/subsystems/module/Module.java | 4 +- .../frc/robot/subsystems/module/ModuleIO.java | 6 ++- .../robot/subsystems/module/ModuleIOReal.java | 48 +++++++++++++++-- .../robot/subsystems/module/ModuleIOSim.java | 52 ++++++++++++++++--- 5 files changed, 97 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4594a8d6..f208b474 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; import frc.robot.constants.RobotConstants; +import frc.robot.constants.RobotPIDConstants; import frc.robot.constants.RobotConstants.RobotTypes; import frc.robot.subsystems.module.ModuleInfo; import frc.robot.util.robotswitcher.Switchable; @@ -44,6 +45,7 @@ public static enum PivotId { public static final double maxOmega = (maxSpeed / maxNorm); public static final double accelLimit = 20; public static final double deaccelLimit = 11; + public static final double maxSteerRateRadiansPerSecond = 10 / RobotPIDConstants.steerFF.kV; public static final double wheelRadius = Units.inchesToMeters(2); diff --git a/src/main/java/frc/robot/subsystems/module/Module.java b/src/main/java/frc/robot/subsystems/module/Module.java index bd38ae24..3b8a73eb 100644 --- a/src/main/java/frc/robot/subsystems/module/Module.java +++ b/src/main/java/frc/robot/subsystems/module/Module.java @@ -48,10 +48,10 @@ public Module(ModuleIO io, PivotId id) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Drive/Modules/" + id, inputs); - Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.velocitySetpoint(), Units.MetersPerSecond); + Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.driveVelocitySetpoint(), Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/currentVel", getVelocity(), Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/velocityError", - Math.abs(io.velocitySetpoint() - getVelocity()), Units.MetersPerSecond); + Math.abs(io.driveVelocitySetpoint() - getVelocity()), Units.MetersPerSecond); } public void setDesiredStateMetersPerSecond(SwerveModuleState state) { diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/module/ModuleIO.java index 7d713737..172b5c5c 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIO.java @@ -52,7 +52,11 @@ public default void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { default void close() { } - public default double velocitySetpoint() { + public default double driveVelocitySetpoint() { + return 0.0; + } + + public default double steerVelocitySetpoint() { return 0.0; } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index 56e5b88b..34db0d84 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -18,7 +18,8 @@ import frc.robot.util.spark.SparkConstants; public class ModuleIOReal implements ModuleIO { - private double velocitySetpoint = 0; + private double driveVelocitySetpoint = 0; + private double steerVelocitySetpoint = 0; private final Queue timestampQueue; private final Queue drivePositionQueue; private final Queue turnPositionQueue; @@ -34,11 +35,16 @@ public class ModuleIOReal implements ModuleIO { private final PIDController drivePID; private final SimpleMotorFeedforward driveFF; private final PIDController steerPID; + private final SimpleMotorFeedforward steerFF; + + private Rotation2d lastSteerSetpoint = new Rotation2d(); + private int lastSteerSetpointCounter = 0; public ModuleIOReal(ModuleInfo id) { drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString()); driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString()); steerPID = RobotPIDConstants.constructPID(RobotPIDConstants.steerPid, "steerPID" + id.id.toString()); + steerFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.steerFF, "steerFF" + id.id.toString()); driveSpark = SparkConfigurer.configSparkFlex(id.driveID, SparkConstants.driveConfig); steerSpark = SparkConfigurer.configSparkMax(id.steerID, SparkConstants.steerConfig); timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); @@ -104,6 +110,8 @@ public void updateInputs(ModuleIOInputs inputs) { driveVelocityQueue.clear(); // inputs.rawEncoderValue = steeringEncoder.getRawValue(); + + lastSteerSetpointCounter = Math.max(0, lastSteerSetpointCounter - 1); } @Override @@ -111,7 +119,7 @@ public void setDriveVelocity(double velocity, ModuleIOInputs inputs) { double pidSpeed = driveFF.calculate(velocity); pidSpeed += drivePID.calculate(inputs.driveVelocityMetersPerSecond, velocity); setDriveVoltage(pidSpeed); - velocitySetpoint = velocity; + driveVelocitySetpoint = velocity; } @Override @@ -123,7 +131,23 @@ public void setDriveVoltage(double voltage) { public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { Rotation2d delta = angle.minus(Rotation2d.fromDegrees(inputs.steerAngleDegrees)); double sin = Math.sin(delta.getRadians()); - setSteerVoltage(steerPID.calculate(sin, 0) * 6); + + // calculate setpoint velocity using last input + double angularVelocityRadPerSec; + if (!isLastSteerSetpointValid()) + angularVelocityRadPerSec = 0; // do not set velocity if estimate is stale + else { // if last setpoint is fresh (last robot loop), approximate derivative of setpoint + angularVelocityRadPerSec = angle.minus(lastSteerSetpoint).getRadians() / 0.02; + } // clamp velocity setpoint + angularVelocityRadPerSec = MathUtil.clamp(angularVelocityRadPerSec, + -DriveConstants.maxSteerRateRadiansPerSecond, DriveConstants.maxSteerRateRadiansPerSecond); + steerVelocitySetpoint = angularVelocityRadPerSec; + // update last setpoint + updateLastSteerSetpoint(angle); + + // compute output voltage + double voltage = steerFF.calculate(angularVelocityRadPerSec) + steerPID.calculate(sin, 0) * 6; + setSteerVoltage(voltage); } @Override @@ -132,7 +156,21 @@ public void setSteerVoltage(double voltage) { } @Override - public double velocitySetpoint() { - return velocitySetpoint; + public double driveVelocitySetpoint() { + return driveVelocitySetpoint; + } + + @Override + public double steerVelocitySetpoint() { + return steerVelocitySetpoint; + } + + private void updateLastSteerSetpoint(Rotation2d lastSteerSetpoint) { + this.lastSteerSetpoint = lastSteerSetpoint; + lastSteerSetpointCounter = 2; + } + + private boolean isLastSteerSetpointValid() { + return lastSteerSetpointCounter > 0; } } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index 5d673bd2..dd4ab32c 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java @@ -12,7 +12,8 @@ import frc.robot.subsystems.drive.DriveConstants; public class ModuleIOSim implements ModuleIO { - private double velocitySetpoint = 0; + private double driveVelocitySetpoint = 0; + private double steerVelocitySetpoint = 0; private final DCMotorSim driveSim; private final DCMotorSim turnSim; private double driveAppliedVolts = 0.0; @@ -21,11 +22,16 @@ public class ModuleIOSim implements ModuleIO { private final PIDController drivePID; private final SimpleMotorFeedforward driveFF; private final PIDController steerPID; + private final SimpleMotorFeedforward steerFF; + + private Rotation2d lastSteerSetpoint = new Rotation2d(); + private int lastSteerSetpointCounter = 0; public ModuleIOSim(ModuleInfo id) { drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString()); driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString()); steerPID = RobotPIDConstants.constructPID(RobotPIDConstants.steerPid, "steerPID" + id.id.toString()); + steerFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.steerFF, "steerFF" + id.id.toString()); DCMotor driveGearbox = DCMotor.getNeoVortex(1); DCMotor turnGearbox = DCMotor.getNeo550(1); driveSim = new DCMotorSim( @@ -41,7 +47,7 @@ public void setDriveVelocity(double velocity, ModuleIOInputs inputs) { double pidSpeed = driveFF.calculate(velocity); pidSpeed += drivePID.calculate(inputs.driveVelocityMetersPerSecond, velocity); setDriveVoltage(pidSpeed); - velocitySetpoint = velocity; + driveVelocitySetpoint = velocity; } @Override @@ -53,12 +59,28 @@ public void setDriveVoltage(double voltage) { public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { Rotation2d delta = angle.minus(Rotation2d.fromDegrees(inputs.steerAngleDegrees)); double sin = Math.sin(delta.getRadians()); - setSteerVoltage(MathUtil.clamp(steerPID.calculate(sin, 0) * 12, -12, 12)); + + // calculate setpoint velocity using last input + double angularVelocityRadPerSec; + if (!isLastSteerSetpointValid()) + angularVelocityRadPerSec = 0; // do not set velocity if estimate is stale + else { // if last setpoint is fresh (last robot loop), approximate derivative of setpoint + angularVelocityRadPerSec = angle.minus(lastSteerSetpoint).getRadians() / 0.02; + } // clamp velocity setpoint + angularVelocityRadPerSec = MathUtil.clamp(angularVelocityRadPerSec, + -DriveConstants.maxSteerRateRadiansPerSecond, DriveConstants.maxSteerRateRadiansPerSecond); + steerVelocitySetpoint = angularVelocityRadPerSec; + // update last setpoint + updateLastSteerSetpoint(angle); + + // compute output voltage + double voltage = steerFF.calculate(angularVelocityRadPerSec) + steerPID.calculate(sin, 0) * 12; + setSteerVoltage(voltage); } @Override public void setSteerVoltage(double voltage) { - turnAppliedVolts = -voltage; + turnAppliedVolts = -MathUtil.clamp(voltage, -12, 12); } @Override @@ -85,11 +107,27 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsMeters = new double[]{inputs.drivePositionMeters}; inputs.odometryTurnPositions = new Rotation2d[]{Rotation2d.fromDegrees(inputs.steerAngleDegrees)}; - inputs.driveVelocities = new double[]{inputs.driveVelocityMetersPerSecond}; + inputs.driveVelocities = new double[]{inputs.driveVelocityMetersPerSecond}; + + lastSteerSetpointCounter = Math.max(0, lastSteerSetpointCounter - 1); } @Override - public double velocitySetpoint() { - return velocitySetpoint; + public double driveVelocitySetpoint() { + return driveVelocitySetpoint; + } + + @Override + public double steerVelocitySetpoint() { + return steerVelocitySetpoint; + } + + private void updateLastSteerSetpoint(Rotation2d lastSteerSetpoint) { + this.lastSteerSetpoint = lastSteerSetpoint; + lastSteerSetpointCounter = 2; + } + + private boolean isLastSteerSetpointValid() { + return lastSteerSetpointCounter > 0; } } From 527368d005af430123b148f7c6eee1220731dada Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 7 Oct 2026 22:43:15 -0400 Subject: [PATCH 09/24] Logging and setting in DriveConstants --- .../robot/subsystems/drive/DriveConstants.java | 1 + .../frc/robot/subsystems/module/Module.java | 1 + .../robot/subsystems/module/ModuleIOReal.java | 17 ++++++++++++++--- .../robot/subsystems/module/ModuleIOSim.java | 17 ++++++++++++++--- 4 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f208b474..c4649d97 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -46,6 +46,7 @@ public static enum PivotId { public static final double accelLimit = 20; public static final double deaccelLimit = 11; public static final double maxSteerRateRadiansPerSecond = 10 / RobotPIDConstants.steerFF.kV; + public static final boolean useSteerVelocitySetpoint = true; public static final double wheelRadius = Units.inchesToMeters(2); diff --git a/src/main/java/frc/robot/subsystems/module/Module.java b/src/main/java/frc/robot/subsystems/module/Module.java index 3b8a73eb..f069858f 100644 --- a/src/main/java/frc/robot/subsystems/module/Module.java +++ b/src/main/java/frc/robot/subsystems/module/Module.java @@ -52,6 +52,7 @@ public void periodic() { Logger.recordOutput("Subsystems/Module/" + id + "/currentVel", getVelocity(), Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/velocityError", Math.abs(io.driveVelocitySetpoint() - getVelocity()), Units.MetersPerSecond); + Logger.recordOutput("Subsystems/Module/" + id + "/steerSetpointVel", io.steerVelocitySetpoint(), Units.RadiansPerSecond); } public void setDesiredStateMetersPerSecond(SwerveModuleState state) { diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index 34db0d84..588ad043 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -2,6 +2,8 @@ import java.util.Queue; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; @@ -17,6 +19,8 @@ import frc.robot.util.spark.SparkConfigurer; import frc.robot.util.spark.SparkConstants; +import static frc.robot.subsystems.drive.DriveConstants.useSteerVelocitySetpoint; + public class ModuleIOReal implements ModuleIO { private double driveVelocitySetpoint = 0; private double steerVelocitySetpoint = 0; @@ -40,6 +44,8 @@ public class ModuleIOReal implements ModuleIO { private Rotation2d lastSteerSetpoint = new Rotation2d(); private int lastSteerSetpointCounter = 0; + private final String logPath; + public ModuleIOReal(ModuleInfo id) { drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString()); driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString()); @@ -66,6 +72,8 @@ public ModuleIOReal(ModuleInfo id) { turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(steerSpark, () -> steeringEncoder.getDegrees() % 360); + + logPath = "Subsystems/Module/" + id.id.toString(); } @Override @@ -134,7 +142,7 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { // calculate setpoint velocity using last input double angularVelocityRadPerSec; - if (!isLastSteerSetpointValid()) + if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint) angularVelocityRadPerSec = 0; // do not set velocity if estimate is stale else { // if last setpoint is fresh (last robot loop), approximate derivative of setpoint angularVelocityRadPerSec = angle.minus(lastSteerSetpoint).getRadians() / 0.02; @@ -146,8 +154,11 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { updateLastSteerSetpoint(angle); // compute output voltage - double voltage = steerFF.calculate(angularVelocityRadPerSec) + steerPID.calculate(sin, 0) * 6; - setSteerVoltage(voltage); + double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); + double pidVoltage = steerPID.calculate(sin, 0) * 6; + Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); + Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); + setSteerVoltage(ffVoltage + pidVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index dd4ab32c..b5e53f54 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java @@ -11,6 +11,10 @@ import frc.robot.constants.RobotPIDConstants; import frc.robot.subsystems.drive.DriveConstants; +import static frc.robot.subsystems.drive.DriveConstants.useSteerVelocitySetpoint; + +import org.littletonrobotics.junction.Logger; + public class ModuleIOSim implements ModuleIO { private double driveVelocitySetpoint = 0; private double steerVelocitySetpoint = 0; @@ -27,6 +31,8 @@ public class ModuleIOSim implements ModuleIO { private Rotation2d lastSteerSetpoint = new Rotation2d(); private int lastSteerSetpointCounter = 0; + private final String logPath; + public ModuleIOSim(ModuleInfo id) { drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString()); driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString()); @@ -40,6 +46,8 @@ public ModuleIOSim(ModuleInfo id) { turnSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(turnGearbox, 0.002174375, DriveConstants.steerGearRatio), turnGearbox); + + logPath = "Subsystems/Module/" + id.id.toString(); } @Override @@ -62,7 +70,7 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { // calculate setpoint velocity using last input double angularVelocityRadPerSec; - if (!isLastSteerSetpointValid()) + if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint) angularVelocityRadPerSec = 0; // do not set velocity if estimate is stale else { // if last setpoint is fresh (last robot loop), approximate derivative of setpoint angularVelocityRadPerSec = angle.minus(lastSteerSetpoint).getRadians() / 0.02; @@ -74,8 +82,11 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { updateLastSteerSetpoint(angle); // compute output voltage - double voltage = steerFF.calculate(angularVelocityRadPerSec) + steerPID.calculate(sin, 0) * 12; - setSteerVoltage(voltage); + double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); + double pidVoltage = steerPID.calculate(sin, 0) * 12; + Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); + Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); + setSteerVoltage(ffVoltage + pidVoltage); } @Override From e3ebc328e694a1faa96b7cc363253b4a8fecf104 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 7 Oct 2026 23:18:19 -0400 Subject: [PATCH 10/24] Filtering for discontinuities, velocity direction, and error deadband --- .../subsystems/drive/DriveConstants.java | 1 + .../frc/robot/subsystems/module/Module.java | 6 ++-- .../robot/subsystems/module/ModuleIOReal.java | 29 +++++++++++------ .../robot/subsystems/module/ModuleIOSim.java | 31 +++++++++++++------ 4 files changed, 46 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index c4649d97..0c367a34 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -46,6 +46,7 @@ public static enum PivotId { public static final double accelLimit = 20; public static final double deaccelLimit = 11; public static final double maxSteerRateRadiansPerSecond = 10 / RobotPIDConstants.steerFF.kV; + public static final double steerSetpointContinuityDeltaRadPerSec = (Math.PI / 8) / 0.02; public static final boolean useSteerVelocitySetpoint = true; public static final double wheelRadius = Units.inchesToMeters(2); diff --git a/src/main/java/frc/robot/subsystems/module/Module.java b/src/main/java/frc/robot/subsystems/module/Module.java index f069858f..649accdf 100644 --- a/src/main/java/frc/robot/subsystems/module/Module.java +++ b/src/main/java/frc/robot/subsystems/module/Module.java @@ -48,11 +48,13 @@ public Module(ModuleIO io, PivotId id) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Drive/Modules/" + id, inputs); - Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.driveVelocitySetpoint(), Units.MetersPerSecond); + Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.driveVelocitySetpoint(), + Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/currentVel", getVelocity(), Units.MetersPerSecond); Logger.recordOutput("Subsystems/Module/" + id + "/velocityError", Math.abs(io.driveVelocitySetpoint() - getVelocity()), Units.MetersPerSecond); - Logger.recordOutput("Subsystems/Module/" + id + "/steerSetpointVel", io.steerVelocitySetpoint(), Units.RadiansPerSecond); + Logger.recordOutput("Subsystems/Module/" + id + "/steerSetpointVel", io.steerVelocitySetpoint(), + Units.RadiansPerSecond); } public void setDesiredStateMetersPerSecond(SwerveModuleState state) { diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index 588ad043..e2e79229 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; import frc.robot.constants.RobotPIDConstants; import frc.robot.sensors.odometry.SparkOdometryThread; import frc.robot.sensors.resolvers.ResolverPWM; @@ -72,7 +73,7 @@ public ModuleIOReal(ModuleInfo id) { turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(steerSpark, () -> steeringEncoder.getDegrees() % 360); - + logPath = "Subsystems/Module/" + id.id.toString(); } @@ -141,23 +142,33 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { double sin = Math.sin(delta.getRadians()); // calculate setpoint velocity using last input - double angularVelocityRadPerSec; + double setpointDerivative; if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint) - angularVelocityRadPerSec = 0; // do not set velocity if estimate is stale - else { // if last setpoint is fresh (last robot loop), approximate derivative of setpoint - angularVelocityRadPerSec = angle.minus(lastSteerSetpoint).getRadians() / 0.02; + setpointDerivative = 0; // do not set velocity if estimate is stale + else { // if last setpoint is fresh (last robot loop), approximate derivative of + // setpoint + setpointDerivative = angle.minus(lastSteerSetpoint).getRadians() / 0.02; } // clamp velocity setpoint - angularVelocityRadPerSec = MathUtil.clamp(angularVelocityRadPerSec, - -DriveConstants.maxSteerRateRadiansPerSecond, DriveConstants.maxSteerRateRadiansPerSecond); + Logger.recordOutput(logPath + "/steerSetpointDerivative", setpointDerivative, Units.RadiansPerSecond); + + double angularVelocityRadPerSec; + // force velocity setpoint to zero if setpoint discontinuous + if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec)) + angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -DriveConstants.maxSteerRateRadiansPerSecond, + DriveConstants.maxSteerRateRadiansPerSecond); + else + angularVelocityRadPerSec = 0; + angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * Math.signum(sin); + steerVelocitySetpoint = angularVelocityRadPerSec; // update last setpoint updateLastSteerSetpoint(angle); // compute output voltage - double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); double pidVoltage = steerPID.calculate(sin, 0) * 6; - Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); + double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); + Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); setSteerVoltage(ffVoltage + pidVoltage); } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index b5e53f54..79c1267f 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.constants.RobotPIDConstants; @@ -46,7 +47,7 @@ public ModuleIOSim(ModuleInfo id) { turnSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(turnGearbox, 0.002174375, DriveConstants.steerGearRatio), turnGearbox); - + logPath = "Subsystems/Module/" + id.id.toString(); } @@ -69,23 +70,33 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { double sin = Math.sin(delta.getRadians()); // calculate setpoint velocity using last input - double angularVelocityRadPerSec; + double setpointDerivative; if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint) - angularVelocityRadPerSec = 0; // do not set velocity if estimate is stale - else { // if last setpoint is fresh (last robot loop), approximate derivative of setpoint - angularVelocityRadPerSec = angle.minus(lastSteerSetpoint).getRadians() / 0.02; + setpointDerivative = 0; // do not set velocity if estimate is stale + else { // if last setpoint is fresh (last robot loop), approximate derivative of + // setpoint + setpointDerivative = angle.minus(lastSteerSetpoint).getRadians() / 0.02; } // clamp velocity setpoint - angularVelocityRadPerSec = MathUtil.clamp(angularVelocityRadPerSec, - -DriveConstants.maxSteerRateRadiansPerSecond, DriveConstants.maxSteerRateRadiansPerSecond); + Logger.recordOutput(logPath + "/steerSetpointDerivative", setpointDerivative, Units.RadiansPerSecond); + + double angularVelocityRadPerSec; + // force velocity setpoint to zero if setpoint discontinuous + if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec)) + angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -DriveConstants.maxSteerRateRadiansPerSecond, + DriveConstants.maxSteerRateRadiansPerSecond); + else + angularVelocityRadPerSec = 0; + angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * -Math.signum(MathUtil.applyDeadband(sin, Math.PI / 36)); + steerVelocitySetpoint = angularVelocityRadPerSec; // update last setpoint updateLastSteerSetpoint(angle); // compute output voltage - double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); double pidVoltage = steerPID.calculate(sin, 0) * 12; - Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); + double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); + Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); setSteerVoltage(ffVoltage + pidVoltage); } @@ -118,7 +129,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsMeters = new double[]{inputs.drivePositionMeters}; inputs.odometryTurnPositions = new Rotation2d[]{Rotation2d.fromDegrees(inputs.steerAngleDegrees)}; - inputs.driveVelocities = new double[]{inputs.driveVelocityMetersPerSecond}; + inputs.driveVelocities = new double[]{inputs.driveVelocityMetersPerSecond}; lastSteerSetpointCounter = Math.max(0, lastSteerSetpointCounter - 1); } From 32f71e3bdc7744485baca62e9a51fd7b798e5731 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sat, 10 Oct 2026 13:02:04 -0400 Subject: [PATCH 11/24] Vararg modules in steer sysid --- .../frc/robot/subsystems/drive/DriveSubsystem.java | 10 +++++----- .../frc/robot/util/sysid/SwerveDriveSysidRoutine.java | 11 +++++------ 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 3073a00f..9e889e0d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -94,16 +94,16 @@ public DriveSubsystem(Gyro gyro) { steerSysIdRoutine = SwerveDriveSysidRoutine .createSteerRoutine( - modules[0], - modules[1], - modules[2], - modules[3], this, new SysIdRoutine.Config( Volts.per(Seconds).of(2), Volts.of(7), Seconds.of(5), - (state) -> Logger.recordOutput("SysIdTestState", state.toString()))); + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + modules[0], + modules[1], + modules[2], + modules[3]); // spotless format try { diff --git a/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java b/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java index 18d0a1cc..b0c1de4a 100644 --- a/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java +++ b/src/main/java/frc/robot/util/sysid/SwerveDriveSysidRoutine.java @@ -35,13 +35,12 @@ public static SysIdRoutine createDriveRoutine(Module fl, Module fr, Module bl, M }, null, subsystem)); } - public static SysIdRoutine createSteerRoutine(Module fl, Module fr, Module bl, Module br, SubsystemBase subsystem, - SysIdRoutine.Config config) { + public static SysIdRoutine createSteerRoutine(SubsystemBase subsystem, + SysIdRoutine.Config config, Module... modules) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism((Voltage voltage) -> { - fl.setSteerVoltage(voltage.in(Volts)); - fr.setSteerVoltage(voltage.in(Volts)); - bl.setSteerVoltage(voltage.in(Volts)); - br.setSteerVoltage(voltage.in(Volts)); + for (Module module : modules) { + module.setSteerVoltage(voltage.in(Volts)); + } }, null, subsystem)); } } From 89f33574fbf7f3d1151e5045bf8cfd51f8b99aee Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sat, 10 Oct 2026 14:35:57 -0400 Subject: [PATCH 12/24] changes --- .../robot/subsystems/drive/DriveConstants.java | 6 ++++-- .../robot/subsystems/drive/DriveSubsystem.java | 16 +++++++++++++--- .../drive/weights/JoystickDriveWeight.java | 4 ++-- .../robot/subsystems/module/ModuleIOReal.java | 6 ++++-- 4 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 0c367a34..1f487fcf 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -46,15 +46,17 @@ public static enum PivotId { public static final double accelLimit = 20; public static final double deaccelLimit = 11; public static final double maxSteerRateRadiansPerSecond = 10 / RobotPIDConstants.steerFF.kV; - public static final double steerSetpointContinuityDeltaRadPerSec = (Math.PI / 8) / 0.02; + public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / 0.02; public static final boolean useSteerVelocitySetpoint = true; + public static final double defaultDreamLevel = 1; + public static final double wheelRadius = Units.inchesToMeters(2); public static final double initalSlope = 3.125; public static final double finalSlope = 4.375; - public static final double maxSteerSpeed = 50; // rad per second + public static final double maxSteerSpeed = Double.POSITIVE_INFINITY; // rad per second public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 9e889e0d..b65730e2 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -126,7 +126,7 @@ public void configureBLine() { FollowPath.setPoseLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond())); FollowPath.setTranslationListLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond())); this.pathBuilder = new FollowPath.Builder((SubsystemBase) this, () -> RobotOdometry.instance.getPose("Main"), - this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, 3, () -> false), + this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, () -> false), new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1)) .withDefaultShouldFlip().withPoseReset((pose) -> { CommandScheduler.getInstance().schedule(new InstantCommand(() -> { @@ -209,6 +209,16 @@ public SwerveModulePosition[] getModulePositions() { return states; } + public void resetLoggedError() { + for (Module module : modules) { + module.resetLoggedError(); + } + } + + public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, BooleanSupplier limitSpeeds) { + runVelocity(speeds, fieldCentric, DriveConstants.defaultDreamLevel, limitSpeeds); + } + public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dreamLevel, BooleanSupplier limitSpeeds) { @@ -312,11 +322,11 @@ public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { - return new RunCommand(() -> runVelocity(speeds.get(), true, 1, limitSpeeds), this).finallyDo(() -> stop()); + return new RunCommand(() -> runVelocity(speeds.get(), true, limitSpeeds), this).finallyDo(() -> stop()); } public Consumer runVelocityConsumer() { - return (speeds) -> runVelocity(speeds, true, 3, () -> false); + return (speeds) -> runVelocity(speeds, true, () -> false); } public static ModuleIO getIOByMode(ModuleInfo modInfo) { diff --git a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java index b4eb9cd8..096a52ba 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java @@ -68,8 +68,8 @@ public ChassisSpeeds getSpeeds() { linearVelocity = linearVelocity.div(linearVelocity.getNorm()); } omega = MathUtil.clamp(omega, -1, 1); - double xyMult = 0.90; - double omegaMult = 0.7; + double xyMult = 1; // 0.90; + double omegaMult = 1; // 0.7; if (slowMode.getAsBoolean()) { xyMult = 0.3; omegaMult = 0.4; diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index e2e79229..aeb418cd 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -140,6 +140,9 @@ public void setDriveVoltage(double voltage) { public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { Rotation2d delta = angle.minus(Rotation2d.fromDegrees(inputs.steerAngleDegrees)); double sin = Math.sin(delta.getRadians()); + double pidVoltage = steerPID.calculate(sin, 0) * 6; + + Logger.recordOutput(logPath + "/errorRadians", (Math.abs(inputs.steerAngleDegrees - angle.getDegrees()) % 180) * Math.PI / 180); // calculate setpoint velocity using last input double setpointDerivative; @@ -158,14 +161,13 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { DriveConstants.maxSteerRateRadiansPerSecond); else angularVelocityRadPerSec = 0; - angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * Math.signum(sin); + angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * Math.max(0, Math.signum(pidVoltage)); steerVelocitySetpoint = angularVelocityRadPerSec; // update last setpoint updateLastSteerSetpoint(angle); // compute output voltage - double pidVoltage = steerPID.calculate(sin, 0) * 6; double ffVoltage = steerFF.calculate(angularVelocityRadPerSec); Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage); Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage); From caf87a9b70e988f309aceaea6bc8e533af3ad850 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sat, 10 Oct 2026 15:24:58 -0400 Subject: [PATCH 13/24] Fix --- .../java/frc/robot/subsystems/drive/DriveSubsystem.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index b65730e2..b60c2d11 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -209,12 +209,6 @@ public SwerveModulePosition[] getModulePositions() { return states; } - public void resetLoggedError() { - for (Module module : modules) { - module.resetLoggedError(); - } - } - public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, BooleanSupplier limitSpeeds) { runVelocity(speeds, fieldCentric, DriveConstants.defaultDreamLevel, limitSpeeds); } From 0970a8ec1f819fca49096389ec207950a125d29d Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Mon, 12 Oct 2026 23:36:38 -0400 Subject: [PATCH 14/24] Bypass setpoint generator code --- src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index b60c2d11..bc6ddc90 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -236,6 +236,8 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream speedsOptimized, // The desired target speeds 0.02 // The loop time of the robot code, in seconds ); + // replace above w/ below to bypass generator + // previousSetpoint = new SwerveSetpoint(speedsOptimized, DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), DriveFeedforwards.zeros(4)); Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates()); Logger.recordOutput("Drive/SwerveStates/Input", DriveConstants.kinematics.toSwerveModuleStates(speeds)); Logger.recordOutput("Drive/SwerveStates/DoubleCone", From f9993cab941846acd2e1d45925a82afbc7a1a07c Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 14 Oct 2026 18:50:49 -0400 Subject: [PATCH 15/24] Removed generator for time being --- .../frc/robot/subsystems/drive/DriveSubsystem.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index bc6ddc90..8db62a67 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -232,12 +232,12 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream doubleCone.vyMetersPerSecond * DriveConstants.maxSpeed * scale, doubleCone.omegaRadiansPerSecond * DriveConstants.maxOmega * scale); - previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint - speedsOptimized, // The desired target speeds - 0.02 // The loop time of the robot code, in seconds - ); + // previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint + // speedsOptimized, // The desired target speeds + // 0.02 // The loop time of the robot code, in seconds + // ); // replace above w/ below to bypass generator - // previousSetpoint = new SwerveSetpoint(speedsOptimized, DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), DriveFeedforwards.zeros(4)); + previousSetpoint = new SwerveSetpoint(speedsOptimized, DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), DriveFeedforwards.zeros(4)); Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates()); Logger.recordOutput("Drive/SwerveStates/Input", DriveConstants.kinematics.toSwerveModuleStates(speeds)); Logger.recordOutput("Drive/SwerveStates/DoubleCone", From c281d46c5b959fdb64483f723ce94d090d377717 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 14 Oct 2026 20:11:11 -0400 Subject: [PATCH 16/24] Starting decel limiting --- src/main/java/frc/robot/constants/RobotConstants.java | 2 +- .../java/frc/robot/subsystems/drive/DriveConstants.java | 8 ++++++-- src/main/java/frc/robot/subsystems/module/Module.java | 4 ++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 8cc393d9..5d8efe23 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -189,7 +189,7 @@ public static class WarningThresholdConstants { public class AutopilotConstants { private static final APConstraints kConstraints = new APConstraints() - .withAcceleration(DriveConstants.accelLimit).withVelocity(DriveConstants.maxSpeed).withJerk(2.0); + .withAcceleration(DriveConstants.moduleAccelLimit).withVelocity(DriveConstants.maxSpeed).withJerk(2.0); private static final APProfile kProfile = new APProfile(kConstraints).withErrorXY(Centimeters.of(2)) .withErrorTheta(Degrees.of(0.5)).withBeelineRadius(Centimeters.of(8)); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 480fbdef..d7f1f736 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -43,8 +43,12 @@ public static enum PivotId { public static final double maxSpeed = 4.25; public static final double maxNorm = DriveSubsystem.computeMaxNorm(DriveConstants.positions, new Translation2d()); public static final double maxOmega = (maxSpeed / maxNorm); - public static final double accelLimit = 20; - public static final double deaccelLimit = 11; + public static final double maxTranslationalDecel = 10; + public static final double maxRotationalDecel = Double.POSITIVE_INFINITY; + + public static final double moduleAccelLimit = 20; + public static final double moduleDecelLimit = 11; + public static final double maxSteerRateRadiansPerSecond = 10 / RobotPIDConstants.steerFF.kV; public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / 0.02; public static final boolean useSteerVelocitySetpoint = true; diff --git a/src/main/java/frc/robot/subsystems/module/Module.java b/src/main/java/frc/robot/subsystems/module/Module.java index 649accdf..8e38d7b9 100644 --- a/src/main/java/frc/robot/subsystems/module/Module.java +++ b/src/main/java/frc/robot/subsystems/module/Module.java @@ -20,8 +20,8 @@ public class Module { PivotId id; ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - SlewRateLimiter accelLimiter = new SlewRateLimiter(DriveConstants.accelLimit); - SlewRateLimiter deaccelLimiter = new SlewRateLimiter(DriveConstants.deaccelLimit); + SlewRateLimiter accelLimiter = new SlewRateLimiter(DriveConstants.moduleAccelLimit); + SlewRateLimiter deaccelLimiter = new SlewRateLimiter(DriveConstants.moduleDecelLimit); public Module(ModuleIO io, PivotId id) { this.io = io; From bd1d4545c5e32a56b5fd4e0e15fff55083f39c27 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Sat, 17 Oct 2026 16:24:19 -0400 Subject: [PATCH 17/24] fix --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 4 ++-- .../java/frc/robot/subsystems/module/ModuleIOReal.java | 7 +++++-- src/main/java/frc/robot/subsystems/module/ModuleIOSim.java | 7 +++++-- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index e44371db..de2d6f98 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -49,7 +49,6 @@ public static enum PivotId { public static final double moduleAccelLimit = 20; public static final double moduleDecelLimit = 11; - public static final double maxSteerRateRadiansPerSecond = 10 / RobotPIDConstants.steerFF.kV; public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / 0.02; public static final boolean useSteerVelocitySetpoint = true; @@ -84,6 +83,7 @@ public static enum PivotId { public static final ModuleInfo BR = new ModuleInfo(PivotId.BR, SwitchableCANID.of(5).addAlt(RobotConstants.RobotTypes.prime25, 7).get(), SwitchableCANID.of(6).get(), - Switchable.of(3).get(), -135, RobotPIDConstants.steerFFs[3]); + Switchable.of(3).get(), -135, + RobotPIDConstants.steerFFs[3]); } diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index a9ba4506..f7c6fe7d 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -42,6 +42,8 @@ public class ModuleIOReal implements ModuleIO { private final PIDController steerPID; private final SimpleMotorFeedforward steerFF; + private final double maxSteerRateRadiansPerSecond; + private Rotation2d lastSteerSetpoint = new Rotation2d(); private int lastSteerSetpointCounter = 0; @@ -74,6 +76,7 @@ public ModuleIOReal(ModuleInfo id) { () -> steeringEncoder.getDegrees() % 360); logPath = "Subsystems/Module/" + id.id.toString(); + maxSteerRateRadiansPerSecond = 10 / steerFF.getKv(); } @Override @@ -156,8 +159,8 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { double angularVelocityRadPerSec; // force velocity setpoint to zero if setpoint discontinuous if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec)) - angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -DriveConstants.maxSteerRateRadiansPerSecond, - DriveConstants.maxSteerRateRadiansPerSecond); + angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -maxSteerRateRadiansPerSecond, + maxSteerRateRadiansPerSecond); else angularVelocityRadPerSec = 0; angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * Math.max(0, Math.signum(pidVoltage)); diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index a3b0b070..03f1133b 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java @@ -29,6 +29,8 @@ public class ModuleIOSim implements ModuleIO { private final PIDController steerPID; private final SimpleMotorFeedforward steerFF; + private final double maxSteerRateRadiansPerSecond; + private Rotation2d lastSteerSetpoint = new Rotation2d(); private int lastSteerSetpointCounter = 0; @@ -49,6 +51,7 @@ public ModuleIOSim(ModuleInfo id) { turnGearbox); logPath = "Subsystems/Module/" + id.id.toString(); + maxSteerRateRadiansPerSecond = 10 / steerFF.getKv(); } @Override @@ -82,8 +85,8 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { double angularVelocityRadPerSec; // force velocity setpoint to zero if setpoint discontinuous if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec)) - angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -DriveConstants.maxSteerRateRadiansPerSecond, - DriveConstants.maxSteerRateRadiansPerSecond); + angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -maxSteerRateRadiansPerSecond, + maxSteerRateRadiansPerSecond); else angularVelocityRadPerSec = 0; angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * -Math.signum(MathUtil.applyDeadband(sin, Math.PI / 36)); From f361f81ff6f90a52b8985ac38021725b323be481 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:17:15 -0400 Subject: [PATCH 18/24] commit by jake --- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/constants/RobotConstants.java | 2 +- .../frc/robot/subsystems/drive/DriveSubsystem.java | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index ede23655..4c7f9999 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -28,7 +28,7 @@ "BAD AUTONS" ], "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 5d8efe23..205f4269 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -27,7 +27,7 @@ public class RobotConstants { public class RobotInformation { - public static final RobotType robot = RobotTypes.deux26; + public static final RobotType robot = RobotTypes.prime26; } public class RobotTypes { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index dc5ca5c3..a6dd4840 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -245,12 +245,12 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream doubleCone.vyMetersPerSecond * DriveConstants.maxSpeed * scale, doubleCone.omegaRadiansPerSecond * DriveConstants.maxOmega * scale); - // previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint - // speedsOptimized, // The desired target speeds - // 0.02 // The loop time of the robot code, in seconds - // ); + previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint + speedsOptimized, // The desired target speeds + 0.02 // The loop time of the robot code, in seconds + ); // replace above w/ below to bypass generator - previousSetpoint = new SwerveSetpoint(speedsOptimized, DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), DriveFeedforwards.zeros(4)); + // previousSetpoint = new SwerveSetpoint(speedsOptimized, DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), DriveFeedforwards.zeros(4)); Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates()); From 397a1f8916a8fe686106fbc77bc455d4fb51ed27 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:42:30 -0400 Subject: [PATCH 19/24] commit --- .../frc/robot/constants/RobotConstants.java | 9 ++++++++- .../robot/subsystems/drive/DriveConstants.java | 6 +++--- .../robot/subsystems/drive/DriveSubsystem.java | 18 +++++++++++++----- .../drive/weights/JoystickDriveWeight.java | 5 +++-- .../robot/subsystems/module/ModuleIOReal.java | 3 ++- .../robot/subsystems/module/ModuleIOSim.java | 3 ++- .../robot/util/robotswitcher/RobotType.java | 7 +++++++ 7 files changed, 38 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 205f4269..737dccad 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -48,7 +48,14 @@ public class RobotTypes { intakeRollerSubsystem, turretSubsystem, shooterSubsystem, spindexerSubsystem, kickerSubsystem, intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) .addAprilTagCamera(CameraConstants.primeRightCamera) - .addAprilTagCamera(CameraConstants.primeBackCamera); + .addAprilTagCamera(CameraConstants.primeBackCamera).setUseTunedSwerve(true); + + public static final RobotType prime26Untuned = new RobotType("Prime26Untuned", driveSubsystem, hoodSubsystem, + intakeRollerSubsystem, turretSubsystem, shooterSubsystem, spindexerSubsystem, kickerSubsystem, + intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) + .addAprilTagCamera(CameraConstants.primeRightCamera) + .addAprilTagCamera(CameraConstants.primeBackCamera).setUseTunedSwerve(false); + public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem, // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, // shooterSubsystem, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index de2d6f98..6eb9ecfc 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -49,7 +49,8 @@ public static enum PivotId { public static final double moduleAccelLimit = 20; public static final double moduleDecelLimit = 11; - public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / 0.02; + public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / + // 0.02; public static final boolean useSteerVelocitySetpoint = true; public static final double defaultDreamLevel = 1; @@ -83,7 +84,6 @@ public static enum PivotId { public static final ModuleInfo BR = new ModuleInfo(PivotId.BR, SwitchableCANID.of(5).addAlt(RobotConstants.RobotTypes.prime25, 7).get(), SwitchableCANID.of(6).get(), - Switchable.of(3).get(), -135, - RobotPIDConstants.steerFFs[3]); + Switchable.of(3).get(), -135, RobotPIDConstants.steerFFs[3]); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index a6dd4840..7f609269 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; import frc.robot.constants.RobotConstants; +import frc.robot.constants.RobotConstants.RobotInformation; import frc.robot.constants.RobotConstants.RobotTypes; import frc.robot.lib.BLine.FollowPath; import frc.robot.sensors.gyro.Gyro; @@ -123,7 +124,9 @@ public void configureBLine() { FollowPath.setTranslationListLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond())); this.pathBuilder = new FollowPath.Builder((SubsystemBase) this, () -> RobotOdometry.instance.getPose("Main"), - this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, () -> false), + this::getChassisSpeeds, + (speeds) -> runVelocity(speeds, false, + RobotInformation.robot.isUseTunedSwerve() ? DriveConstants.defaultDreamLevel : 3, () -> false), new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1)) .withDefaultShouldFlip().withPoseReset((pose) -> { CommandScheduler.getInstance().schedule(new InstantCommand(() -> { @@ -244,13 +247,15 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream : new ChassisSpeeds(doubleCone.vxMetersPerSecond * DriveConstants.maxSpeed * scale, doubleCone.vyMetersPerSecond * DriveConstants.maxSpeed * scale, doubleCone.omegaRadiansPerSecond * DriveConstants.maxOmega * scale); - + previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint speedsOptimized, // The desired target speeds 0.02 // The loop time of the robot code, in seconds ); // replace above w/ below to bypass generator - // previousSetpoint = new SwerveSetpoint(speedsOptimized, DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), DriveFeedforwards.zeros(4)); + // previousSetpoint = new SwerveSetpoint(speedsOptimized, + // DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized), + // DriveFeedforwards.zeros(4)); Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates()); @@ -258,7 +263,7 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream Logger.recordOutput("Drive/SwerveStates/DoubleCone", DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)); - + Logger.recordOutput("Drive/SwerveStates/InputStates", DriveConstants.kinematics.toSwerveModuleStates(speeds)); // speedsOptimized = ChassisSpeeds.discretize(speedsOptimized, 0.02); @@ -354,7 +359,10 @@ public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { - return new RunCommand(() -> runVelocity(speeds.get(), true, 3, limitSpeeds), this).finallyDo(() -> stop()); + return new RunCommand( + () -> runVelocity(speeds.get(), true, + RobotInformation.robot.isUseTunedSwerve() ? DriveConstants.defaultDreamLevel : 3, limitSpeeds), + this).finallyDo(() -> stop()); } public Consumer runVelocityConsumer() { diff --git a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java index 096a52ba..bd0ec679 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.RobotState; +import frc.robot.constants.RobotConstants; import frc.robot.sensors.gyro.Gyro; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.helpers.AllianceManager; @@ -68,8 +69,8 @@ public ChassisSpeeds getSpeeds() { linearVelocity = linearVelocity.div(linearVelocity.getNorm()); } omega = MathUtil.clamp(omega, -1, 1); - double xyMult = 1; // 0.90; - double omegaMult = 1; // 0.7; + double xyMult = RobotConstants.RobotInformation.robot.isUseTunedSwerve() ? 1 : 0.9; + double omegaMult = RobotConstants.RobotInformation.robot.isUseTunedSwerve() ? 1 : 0.7; if (slowMode.getAsBoolean()) { xyMult = 0.3; omegaMult = 0.4; diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java index f7c6fe7d..2378063b 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOReal.java @@ -144,7 +144,8 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { double sin = Math.sin(delta.getRadians()); double pidVoltage = steerPID.calculate(sin, 0) * 6; - Logger.recordOutput(logPath + "/errorRadians", (Math.abs(inputs.steerAngleDegrees - angle.getDegrees()) % 180) * Math.PI / 180); + Logger.recordOutput(logPath + "/errorRadians", + (Math.abs(inputs.steerAngleDegrees - angle.getDegrees()) % 180) * Math.PI / 180); // calculate setpoint velocity using last input double setpointDerivative; diff --git a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java index 03f1133b..b3b46107 100644 --- a/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/module/ModuleIOSim.java @@ -89,7 +89,8 @@ public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) { maxSteerRateRadiansPerSecond); else angularVelocityRadPerSec = 0; - angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * -Math.signum(MathUtil.applyDeadband(sin, Math.PI / 36)); + angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) + * -Math.signum(MathUtil.applyDeadband(sin, Math.PI / 36)); steerVelocitySetpoint = angularVelocityRadPerSec; // update last setpoint diff --git a/src/main/java/frc/robot/util/robotswitcher/RobotType.java b/src/main/java/frc/robot/util/robotswitcher/RobotType.java index 3ddae0d2..66a111aa 100644 --- a/src/main/java/frc/robot/util/robotswitcher/RobotType.java +++ b/src/main/java/frc/robot/util/robotswitcher/RobotType.java @@ -5,6 +5,7 @@ import frc.robot.sensors.apriltag.CameraConstant; import frc.robot.util.wrapper.subsystem.SubsystemInfo; +import lombok.Getter; /** * Add more robots by adding to this enum @@ -14,6 +15,8 @@ public class RobotType { List enabled = new ArrayList<>(); String robotName; ArrayList cameras = new ArrayList<>(); + @Getter + boolean useTunedSwerve = false; public RobotType(String robotName, SubsystemInfo... subsystems) { this.robotName = robotName; for (SubsystemInfo sInfo : subsystems) { @@ -36,4 +39,8 @@ public RobotType addAprilTagCamera(CameraConstant cam) { public List getCameras() { return cameras; } + public RobotType setUseTunedSwerve(boolean enable) { + useTunedSwerve = enable; + return this; + } } From 8bbeeb33209c0826f0591e20a6a5c987b2dab924 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:46:29 -0400 Subject: [PATCH 20/24] changes --- src/main/java/frc/robot/constants/RobotConstants.java | 4 ++-- .../frc/robot/subsystems/drive/DriveConstants.java | 1 + .../frc/robot/subsystems/drive/DriveSubsystem.java | 10 ++++------ .../subsystems/drive/weights/JoystickDriveWeight.java | 5 ++--- .../java/frc/robot/util/robotswitcher/RobotType.java | 7 ------- 5 files changed, 9 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 737dccad..63922b49 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -48,13 +48,13 @@ public class RobotTypes { intakeRollerSubsystem, turretSubsystem, shooterSubsystem, spindexerSubsystem, kickerSubsystem, intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) .addAprilTagCamera(CameraConstants.primeRightCamera) - .addAprilTagCamera(CameraConstants.primeBackCamera).setUseTunedSwerve(true); + .addAprilTagCamera(CameraConstants.primeBackCamera); public static final RobotType prime26Untuned = new RobotType("Prime26Untuned", driveSubsystem, hoodSubsystem, intakeRollerSubsystem, turretSubsystem, shooterSubsystem, spindexerSubsystem, kickerSubsystem, intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) .addAprilTagCamera(CameraConstants.primeRightCamera) - .addAprilTagCamera(CameraConstants.primeBackCamera).setUseTunedSwerve(false); + .addAprilTagCamera(CameraConstants.primeBackCamera); public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem, // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 6eb9ecfc..83bb5051 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -86,4 +86,5 @@ public static enum PivotId { SwitchableCANID.of(5).addAlt(RobotConstants.RobotTypes.prime25, 7).get(), SwitchableCANID.of(6).get(), Switchable.of(3).get(), -135, RobotPIDConstants.steerFFs[3]); + public static final boolean useTunedSwerve = true; } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 7f609269..e47125ce 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -37,7 +37,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; import frc.robot.constants.RobotConstants; -import frc.robot.constants.RobotConstants.RobotInformation; import frc.robot.constants.RobotConstants.RobotTypes; import frc.robot.lib.BLine.FollowPath; import frc.robot.sensors.gyro.Gyro; @@ -126,7 +125,7 @@ public void configureBLine() { this.pathBuilder = new FollowPath.Builder((SubsystemBase) this, () -> RobotOdometry.instance.getPose("Main"), this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, - RobotInformation.robot.isUseTunedSwerve() ? DriveConstants.defaultDreamLevel : 3, () -> false), + DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3, () -> false), new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1)) .withDefaultShouldFlip().withPoseReset((pose) -> { CommandScheduler.getInstance().schedule(new InstantCommand(() -> { @@ -359,10 +358,9 @@ public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { - return new RunCommand( - () -> runVelocity(speeds.get(), true, - RobotInformation.robot.isUseTunedSwerve() ? DriveConstants.defaultDreamLevel : 3, limitSpeeds), - this).finallyDo(() -> stop()); + return new RunCommand(() -> runVelocity(speeds.get(), true, + DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3, limitSpeeds), this) + .finallyDo(() -> stop()); } public Consumer runVelocityConsumer() { diff --git a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java index bd0ec679..f3097e1c 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.RobotState; -import frc.robot.constants.RobotConstants; import frc.robot.sensors.gyro.Gyro; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.helpers.AllianceManager; @@ -69,8 +68,8 @@ public ChassisSpeeds getSpeeds() { linearVelocity = linearVelocity.div(linearVelocity.getNorm()); } omega = MathUtil.clamp(omega, -1, 1); - double xyMult = RobotConstants.RobotInformation.robot.isUseTunedSwerve() ? 1 : 0.9; - double omegaMult = RobotConstants.RobotInformation.robot.isUseTunedSwerve() ? 1 : 0.7; + double xyMult = DriveConstants.useTunedSwerve ? 1 : 0.9; + double omegaMult = DriveConstants.useTunedSwerve ? 1 : 0.7; if (slowMode.getAsBoolean()) { xyMult = 0.3; omegaMult = 0.4; diff --git a/src/main/java/frc/robot/util/robotswitcher/RobotType.java b/src/main/java/frc/robot/util/robotswitcher/RobotType.java index 66a111aa..3ddae0d2 100644 --- a/src/main/java/frc/robot/util/robotswitcher/RobotType.java +++ b/src/main/java/frc/robot/util/robotswitcher/RobotType.java @@ -5,7 +5,6 @@ import frc.robot.sensors.apriltag.CameraConstant; import frc.robot.util.wrapper.subsystem.SubsystemInfo; -import lombok.Getter; /** * Add more robots by adding to this enum @@ -15,8 +14,6 @@ public class RobotType { List enabled = new ArrayList<>(); String robotName; ArrayList cameras = new ArrayList<>(); - @Getter - boolean useTunedSwerve = false; public RobotType(String robotName, SubsystemInfo... subsystems) { this.robotName = robotName; for (SubsystemInfo sInfo : subsystems) { @@ -39,8 +36,4 @@ public RobotType addAprilTagCamera(CameraConstant cam) { public List getCameras() { return cameras; } - public RobotType setUseTunedSwerve(boolean enable) { - useTunedSwerve = enable; - return this; - } } From 842c620c1b77276da717585694d85c08394331c9 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:46:56 -0400 Subject: [PATCH 21/24] sorry i forgot --- src/main/java/frc/robot/constants/RobotConstants.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 63922b49..93b42cc0 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -50,12 +50,6 @@ public class RobotTypes { .addAprilTagCamera(CameraConstants.primeRightCamera) .addAprilTagCamera(CameraConstants.primeBackCamera); - public static final RobotType prime26Untuned = new RobotType("Prime26Untuned", driveSubsystem, hoodSubsystem, - intakeRollerSubsystem, turretSubsystem, shooterSubsystem, spindexerSubsystem, kickerSubsystem, - intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera) - .addAprilTagCamera(CameraConstants.primeRightCamera) - .addAprilTagCamera(CameraConstants.primeBackCamera); - public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem, // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem, // shooterSubsystem, From e1933bd2da786a7beebf70b88797f5e44787caa8 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:49:53 -0400 Subject: [PATCH 22/24] stuff i forgot --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 7 +++++-- .../java/frc/robot/subsystems/drive/DriveSubsystem.java | 3 ++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 83bb5051..57634c72 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.useTunedSwerve; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; @@ -51,7 +53,9 @@ public static enum PivotId { public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) / // 0.02; - public static final boolean useSteerVelocitySetpoint = true; + public static final boolean useTunedSwerve = true; + + public static final boolean useSteerVelocitySetpoint = useTunedSwerve; public static final double defaultDreamLevel = 1; @@ -86,5 +90,4 @@ public static enum PivotId { SwitchableCANID.of(5).addAlt(RobotConstants.RobotTypes.prime25, 7).get(), SwitchableCANID.of(6).get(), Switchable.of(3).get(), -135, RobotPIDConstants.steerFFs[3]); - public static final boolean useTunedSwerve = true; } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index e47125ce..7df08057 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -364,7 +364,8 @@ public Command runVelocityCommand(Supplier speeds, BooleanSupplie } public Consumer runVelocityConsumer() { - return (speeds) -> runVelocity(speeds, true, () -> false); + return (speeds) -> runVelocity(speeds, true, + DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3, () -> false); } public static ModuleIO getIOByMode(ModuleInfo modInfo) { From f7bab84dd94ac02c3ead5546d2f60a7fc6b580d9 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:53:14 -0400 Subject: [PATCH 23/24] yayyyy!! --- .../frc/robot/subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/DriveSubsystem.java | 11 +++-------- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 57634c72..88bfb56f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -57,7 +57,7 @@ public static enum PivotId { public static final boolean useSteerVelocitySetpoint = useTunedSwerve; - public static final double defaultDreamLevel = 1; + public static final double defaultDreamLevel = DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3; public static final double wheelRadius = Units.inchesToMeters(2); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 7df08057..cc940abf 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -123,9 +123,7 @@ public void configureBLine() { FollowPath.setTranslationListLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond())); this.pathBuilder = new FollowPath.Builder((SubsystemBase) this, () -> RobotOdometry.instance.getPose("Main"), - this::getChassisSpeeds, - (speeds) -> runVelocity(speeds, false, - DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3, () -> false), + this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, () -> false), new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1)) .withDefaultShouldFlip().withPoseReset((pose) -> { CommandScheduler.getInstance().schedule(new InstantCommand(() -> { @@ -358,14 +356,11 @@ public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) { } public Command runVelocityCommand(Supplier speeds, BooleanSupplier limitSpeeds) { - return new RunCommand(() -> runVelocity(speeds.get(), true, - DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3, limitSpeeds), this) - .finallyDo(() -> stop()); + return new RunCommand(() -> runVelocity(speeds.get(), true, limitSpeeds), this).finallyDo(() -> stop()); } public Consumer runVelocityConsumer() { - return (speeds) -> runVelocity(speeds, true, - DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3, () -> false); + return (speeds) -> runVelocity(speeds, true, () -> false); } public static ModuleIO getIOByMode(ModuleInfo modInfo) { From c74228415357d4f117b2e9f2f298c3480c3ce4e8 Mon Sep 17 00:00:00 2001 From: 1640driverstation Date: Wed, 21 Oct 2026 18:53:52 -0400 Subject: [PATCH 24/24] fix --- src/main/java/frc/robot/subsystems/drive/DriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 88bfb56f..8174d5d5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -57,7 +57,7 @@ public static enum PivotId { public static final boolean useSteerVelocitySetpoint = useTunedSwerve; - public static final double defaultDreamLevel = DriveConstants.useTunedSwerve ? DriveConstants.defaultDreamLevel : 3; + public static final double defaultDreamLevel = DriveConstants.useTunedSwerve ? 1 : 3; public static final double wheelRadius = Units.inchesToMeters(2);