From 06e55c436cfcbb6c360095ddf83b2cd28f9398a0 Mon Sep 17 00:00:00 2001 From: Kgupta011 Date: Sat, 26 Sep 2026 11:54:44 -0400 Subject: [PATCH 1/4] =?UTF-8?q?making=20changes=20to=20see=20if=20we=20can?= =?UTF-8?q?=20speed=20up=20auton=20start=20(=E2=97=8F'=E2=97=A1'=E2=97=8F)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/subsystems/drive/DriveSubsystem.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 6b437179..8ff72f52 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -223,9 +223,12 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream Logger.recordOutput("Drive/SwerveStates/DoubleCone", DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)); // speedsOptimized = ChassisSpeeds.discretize(speedsOptimized, 0.02); + SwerveModuleState[] directStates = DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized); + + Logger.recordOutput("Drive/SwerveStates/DirectStates", directStates); + for (int i = 0; i < 4; i++) { - modules[i].setDesiredStateMetersPerSecond(previousSetpoint.moduleStates()[i]); - // DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)[i]); + modules[i].setDesiredStateMetersPerSecond(directStates[i]); } } From 2dc0bb4bb15295ddb548ade36b7694c49ac18548 Mon Sep 17 00:00:00 2001 From: Kgupta011 Date: Sat, 26 Sep 2026 12:05:46 -0400 Subject: [PATCH 2/4] =?UTF-8?q?fast=20Auton=20change=20=F0=9F=92=A8?= =?UTF-8?q?=F0=9F=92=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7075517..c28d68a4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -416,7 +416,8 @@ private void generateEventTriggers() { } public Command getAutonomousCommand() { - return autonChooser.getAuto(); + Command auto = autonChooser.getAuto(); + return auto.beforeStarting(() -> driveSubsystem.startFastAutonTest()); } public Command getBPLCommand() { From 6d4fed86741785cc522f1948682b368f4b1a4e24 Mon Sep 17 00:00:00 2001 From: Kgupta011 Date: Sat, 26 Sep 2026 12:19:42 -0400 Subject: [PATCH 3/4] fixing an error --- .../subsystems/drive/DriveSubsystem.java | 65 +++++++++++++++---- 1 file changed, 54 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 8ff72f52..0cd4acc1 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -63,6 +63,10 @@ public class DriveSubsystem extends SubsystemPlatform { public Rotation2d totalRot = new Rotation2d(); private FollowPath.Builder pathBuilder; + // STARTUP TEST + private double autonStartTime = -1; + private boolean testingFastAutonStart = false; + // THIS LINE IS ESSENTIAL FOR EVERY SUBSYSTEM public static final SubsystemInfo info = RobotTypes.driveSubsystem; @@ -99,10 +103,9 @@ public DriveSubsystem(Gyro gyro) { e.printStackTrace(); config = null; } - setpointGenerator = new SwerveSetpointGenerator(config, // The robot configuration. This is the same config used - // for generating - // trajectories and running path following commands. - DriveConstants.maxSteerSpeed); + + setpointGenerator = new SwerveSetpointGenerator(config, DriveConstants.maxSteerSpeed); + previousSetpoint = new SwerveSetpoint(getChassisSpeeds(), getActualSwerveStates(), DriveFeedforwards.zeros(4)); } @@ -111,6 +114,7 @@ public void configureBLine() { FollowPath.setDoubleLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond())); 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), new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1)) @@ -126,21 +130,30 @@ public FollowPath.Builder getPathBuilder() { return this.pathBuilder; } + public void startFastAutonTest() { + autonStartTime = -1; + testingFastAutonStart = true; + } + @Override public void periodic() { odometryLock.lock(); + for (var module : modules) { module.periodic(); } + gyro.periodic(); odometryLock.unlock(); double totalDriveCurrent = 0; double totalSteerCurrent = 0; + for (Module module : modules) { totalDriveCurrent += module.getDriveCurrent(); totalSteerCurrent += module.getSteerCurrent(); } + Logger.recordOutput("Subsystems/Drive/totalDriveCurrent", totalDriveCurrent); Logger.recordOutput("Subsystems/Drive/totalSteerCurrent", totalSteerCurrent); } @@ -158,9 +171,11 @@ public Command stopCommand() { @AutoLogOutput(key = "Drive/SwerveStates/Measured") public SwerveModuleState[] getActualSwerveStates() { SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { states[i] = modules[i].getState(); } + return states; } @@ -177,9 +192,11 @@ public double chassisSpeedsMagnitude() { @AutoLogOutput(key = "Drive/SwerveChassisSpeeds/VelocityAngle") public Rotation2d chassisSpeedsAngle() { ChassisSpeeds speeds = getChassisSpeeds(); + if (chassisSpeedsMagnitude() < 0.001) { return new Rotation2d(); } + return new Rotation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond).rotateBy(gyro.getAngleRotation2d()); } @@ -189,9 +206,11 @@ public Module[] getModules() { public SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { states[i] = modules[i].getPosition(); } + return states; } @@ -199,11 +218,13 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream BooleanSupplier limitSpeeds) { double scale = 1; + ChassisSpeeds percent = new ChassisSpeeds(speeds.vxMetersPerSecond / DriveConstants.maxSpeed, speeds.vyMetersPerSecond / DriveConstants.maxSpeed, speeds.omegaRadiansPerSecond / DriveConstants.maxOmega); ChassisSpeeds doubleCone = inceptionMode(percent, new Translation2d(), dreamLevel); + ChassisSpeeds speedsOptimized = fieldCentric ? ChassisSpeeds.fromFieldRelativeSpeeds( new ChassisSpeeds(doubleCone.vxMetersPerSecond * DriveConstants.maxSpeed * scale, @@ -214,21 +235,33 @@ 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, speedsOptimized, 0.02); + Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates()); + Logger.recordOutput("Drive/SwerveStates/Input", DriveConstants.kinematics.toSwerveModuleStates(speeds)); + Logger.recordOutput("Drive/SwerveStates/DoubleCone", DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)); - // speedsOptimized = ChassisSpeeds.discretize(speedsOptimized, 0.02); + SwerveModuleState[] directStates = DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized); Logger.recordOutput("Drive/SwerveStates/DirectStates", directStates); + // STARTUP TEST: + // Only bypass the setpoint generator for the first 0.5 seconds. + if (testingFastAutonStart && autonStartTime < 0) { + autonStartTime = edu.wpi.first.wpilibj.Timer.getFPGATimestamp(); + } + boolean useDirectStates = testingFastAutonStart + && edu.wpi.first.wpilibj.Timer.getFPGATimestamp() - autonStartTime < 0.5; + + SwerveModuleState[] statesToUse = useDirectStates ? directStates : previousSetpoint.moduleStates(); + + Logger.recordOutput("Drive/StartupTest/UsingDirectStates", useDirectStates); + for (int i = 0; i < 4; i++) { - modules[i].setDesiredStateMetersPerSecond(directStates[i]); + modules[i].setDesiredStateMetersPerSecond(statesToUse[i]); } } @@ -242,9 +275,11 @@ private boolean areModulesAtRotations(Rotation2d rotation) { for (int i = 0; i < 4; i++) { if (!MathUtil.isNear(rotation.getRadians(), modules[i].getPosition().angle.getRadians(), Units.degreesToRadians(25))) { + return false; } } + return true; } @@ -265,19 +300,25 @@ public static ChassisSpeeds inceptionMode(ChassisSpeeds speedsPercent, Translati double xSpeed = speedsPercent.vxMetersPerSecond; double ySpeed = speedsPercent.vyMetersPerSecond; double rot = speedsPercent.omegaRadiansPerSecond; + double translationalSpeed = Math.hypot(xSpeed, ySpeed); + double linearRotSpeed = Math.abs(rot * computeMaxNorm(DriveConstants.positions, centerOfRotation)); + double k; + if (linearRotSpeed == 0 || translationalSpeed == 0) { k = 1; } else { k = Math.pow(Math.max(linearRotSpeed, translationalSpeed) / (linearRotSpeed + translationalSpeed), dreamLevel); } + return new ChassisSpeeds(k * xSpeed, k * ySpeed, k * rot); } public static double computeMaxNorm(Translation2d[] translations, Translation2d centerOfRotation) { + return Arrays.stream(translations).map((translation) -> translation.minus(centerOfRotation)) .mapToDouble(Translation2d::getNorm).max() .orElseThrow(() -> new NoSuchElementException("No max norm.")); @@ -292,6 +333,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()); } @@ -302,9 +344,9 @@ public Consumer runVelocityConsumer() { public static ModuleIO getIOByMode(ModuleInfo modInfo) { if (!RobotConstants.RobotInformation.robot.isEnabled(info)) { return new ModuleIO() { - }; } + return switch (Robot.getMode()) { case REAL -> new ModuleIOReal(modInfo); case SIM -> new ModuleIOSim(modInfo); @@ -315,6 +357,7 @@ public static ModuleIO getIOByMode(ModuleInfo modInfo) { @Override public Command dashboardCommand(DoubleSupplier leftJoystickValue, DoubleSupplier rightJoystickValue) { + return Commands.none(); } } From 482e3a67c73f3407b08cd15357ec2ac0461bbfb8 Mon Sep 17 00:00:00 2001 From: Kgupta011 Date: Sat, 26 Sep 2026 12:42:16 -0400 Subject: [PATCH 4/4] =?UTF-8?q?ftc=20vs=20vex=20=20=F0=9F=A4=94?= =?UTF-8?q?=F0=9F=A4=94=F0=9F=A4=94=20which=20one=20is=20better?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 0cd4acc1..fae7b215 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -249,12 +249,11 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream Logger.recordOutput("Drive/SwerveStates/DirectStates", directStates); // STARTUP TEST: - // Only bypass the setpoint generator for the first 0.5 seconds. if (testingFastAutonStart && autonStartTime < 0) { autonStartTime = edu.wpi.first.wpilibj.Timer.getFPGATimestamp(); } boolean useDirectStates = testingFastAutonStart - && edu.wpi.first.wpilibj.Timer.getFPGATimestamp() - autonStartTime < 0.5; + && edu.wpi.first.wpilibj.Timer.getFPGATimestamp() - autonStartTime < 1; SwerveModuleState[] statesToUse = useDirectStates ? directStates : previousSetpoint.moduleStates();