making changes to see if we can speed up auton start (●'◡'●)

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() {

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,21 +206,25 @@ 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;
   }
 
   public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dreamLevel,
       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,18 +235,32 @@ 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:
+    if (testingFastAutonStart && autonStartTime < 0) {
+      autonStartTime = edu.wpi.first.wpilibj.Timer.getFPGATimestamp();
+    }
+    boolean useDirectStates = testingFastAutonStart
+        && edu.wpi.first.wpilibj.Timer.getFPGATimestamp() - autonStartTime < 1;
+
+    SwerveModuleState[] statesToUse = useDirectStates ? directStates : previousSetpoint.moduleStates();
+
+    Logger.recordOutput("Drive/StartupTest/UsingDirectStates", useDirectStates);
+
     for (int i = 0; i < 4; i++) {
-      modules[i].setDesiredStateMetersPerSecond(previousSetpoint.moduleStates()[i]);
-      // DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized)[i]);
+      modules[i].setDesiredStateMetersPerSecond(statesToUse[i]);
     }
   }
 
@@ -239,9 +274,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;
   }
 
@@ -262,19 +299,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."));
@@ -289,6 +332,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
   }
 
   public Command runVelocityCommand(Supplier<ChassisSpeeds> speeds, BooleanSupplier limitSpeeds) {
+
     return new RunCommand(() -> runVelocity(speeds.get(), true, 3, limitSpeeds), this).finallyDo(() -> stop());
   }
 
@@ -299,9 +343,9 @@ public Consumer<ChassisSpeeds> 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);
@@ -312,6 +356,7 @@ public static ModuleIO getIOByMode(ModuleInfo modInfo) {
 
   @Override
   public Command dashboardCommand(DoubleSupplier leftJoystickValue, DoubleSupplier rightJoystickValue) {
+
     return Commands.none();
   }
 }