Merge branch 'main' into 217-add-climbing-parameter-for-superstructur…

…e-goals

build.gradle

@@ -39,7 +39,6 @@ sourceSets {
     }
 }
 
-
 java {
     sourceCompatibility = JavaVersion.VERSION_17
     targetCompatibility = JavaVersion.VERSION_17
@@ -69,6 +68,7 @@ deploy {
 
                 // Static files artifact
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    predeploy << { execute 'rm -rf /home/lvuser/deploy' }
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
                 }

src/main/java/org/littletonrobotics/frc2024/Constants.java

@@ -63,7 +63,7 @@ public static void disableHAL() {
     disableHAL = true;
   }
 
-  /** Checks whether the robot the correct robot is selected when deploying. */
+  /** Checks whether the correct robot is selected when deploying. */
   public static void main(String... args) {
     if (robotType == RobotType.SIMBOT) {
       System.err.println("Cannot deploy, invalid robot selected: " + robotType);

src/main/java/org/littletonrobotics/frc2024/Robot.java

@@ -83,6 +83,7 @@ public class Robot extends LoggedRobot {
           AlertType.WARNING);
   private final Alert sameBatteryAlert =
       new Alert("The battery has not been changed since the last match.", AlertType.WARNING);
+  private final Alert gcAlert = new Alert("Please wait 30 seconds to enable.", AlertType.WARNING);
 
   public static Trigger createTeleopTimeTrigger(DoubleSupplier teleElapsedTime) {
     return new Trigger(
@@ -262,6 +263,9 @@ public void robotPeriodic() {
       Leds.getInstance().lowBatteryAlert = true;
     }
 
+    // GC alert
+    gcAlert.set(Timer.getFPGATimestamp() < 30.0);
+
     // Update battery alert
     String batteryName = batteryNameSubscriber.get();
     Logger.recordOutput("BatteryName", batteryName);

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -520,6 +520,9 @@ private void configureButtonBindings() {
     // Intake Floor
     driver
         .leftTrigger()
+        .and(
+            DriverStation
+                ::isEnabled) // Must be enabled, allowing driver to hold button as soon as auto ends
         .whileTrue(
             superstructure
                 .setGoalCommand(Superstructure.Goal.INTAKE)

src/main/java/org/littletonrobotics/frc2024/RobotState.java

@@ -70,6 +70,7 @@ public record AimingParameters(
     armAngleMap.put(4.0, 27.75);
     armAngleMap.put(4.25, 26.8);
     armAngleMap.put(4.5, 25.6);
+    armAngleMap.put(8.0, 8.8); // Added in with slope of previous two points to make a best guess
   }
 
   @AutoLogOutput @Getter @Setter private double shotCompensationDegrees = 0.0;

src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java

@@ -71,6 +71,9 @@ public static Command trapSequence(
                         Arm.prepareClimbProfileConstraints.get()),
                     rollers.setGoalCommand(Rollers.Goal.SHUFFLE_BACKPACK)),
 
+            // Pre-move arm to climb position to help with alignment and prevent wedging
+            superstructure.setGoalCommand(Superstructure.Goal.POST_PREPARE_TRAP_CLIMB),
+
             // Allow driver to line up and climb
             superstructure.setGoalCommand(Superstructure.Goal.CLIMB),
 

src/main/java/org/littletonrobotics/frc2024/commands/WheelRadiusCharacterization.java

@@ -88,6 +88,7 @@ public void execute() {
 
   @Override
   public void end(boolean interrupted) {
+    drive.endCharacterization();
     if (accumGyroYawRads <= Math.PI * 2.0) {
       System.out.println("Not enough data for characterization");
     } else {

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java

@@ -24,7 +24,7 @@
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Supplier;
 import java.util.stream.IntStream;
-import lombok.Getter;
+import lombok.Setter;
 import lombok.experimental.ExtensionMethod;
 import org.littletonrobotics.frc2024.Constants;
 import org.littletonrobotics.frc2024.RobotState;
@@ -51,7 +51,7 @@ public class Drive extends SubsystemBase {
   private static final LoggedTunableNumber coastMetersPerSecThreshold =
       new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05);
 
-  public enum DriveMode {
+  public static enum DriveMode {
     /** Driving with input from driver joysticks. (Default) */
     TELEOP,
 
@@ -68,6 +68,12 @@ public enum DriveMode {
     WHEEL_RADIUS_CHARACTERIZATION
   }
 
+  public static enum CoastRequest {
+    AUTOMATIC,
+    ALWAYS_BRAKE,
+    ALWAYS_COAST
+  }
+
   @AutoLog
   public static class OdometryTimestampInputs {
     public double[] timestamps = new double[] {};
@@ -93,10 +99,15 @@ public static class OdometryTimestampInputs {
   private boolean modulesOrienting = false;
   private final Timer lastMovementTimer = new Timer();
 
-  @Getter
   @AutoLogOutput(key = "Drive/BrakeModeEnabled")
   private boolean brakeModeEnabled = true;
 
+  @Setter
+  @AutoLogOutput(key = "Drive/CoastRequest")
+  private CoastRequest coastRequest = CoastRequest.AUTOMATIC;
+
+  private boolean lastEnabled = false;
+
   private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
 
   private SwerveSetpoint currentSetpoint =
@@ -122,6 +133,7 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br)
     modules[2] = new Module(bl, 2);
     modules[3] = new Module(br, 3);
     lastMovementTimer.start();
+    setBrakeMode(true);
 
     setpointGenerator =
         SwerveSetpointGenerator.builder()
@@ -220,6 +232,26 @@ public void periodic() {
         .anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) {
       lastMovementTimer.reset();
     }
+    if (DriverStation.isEnabled() && !lastEnabled) {
+      coastRequest = CoastRequest.AUTOMATIC;
+    }
+
+    lastEnabled = DriverStation.isEnabled();
+    switch (coastRequest) {
+      case AUTOMATIC -> {
+        if (DriverStation.isEnabled()) {
+          setBrakeMode(true);
+        } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) {
+          setBrakeMode(false);
+        }
+      }
+      case ALWAYS_BRAKE -> {
+        setBrakeMode(true);
+      }
+      case ALWAYS_COAST -> {
+        setBrakeMode(false);
+      }
+    }
 
     // Run drive based on current mode
     ChassisSpeeds teleopSpeeds = teleopDriveController.update();
@@ -254,16 +286,16 @@ public void periodic() {
       default -> {}
     }
 
-    // Run robot at desiredSpeeds
-    // Generate feasible next setpoint
-    currentSetpoint =
-        setpointGenerator.generateSetpoint(
-            currentModuleLimits, currentSetpoint, desiredSpeeds, Constants.loopPeriodSecs);
-
-    // run modules
+    // Run modules
     if (currentDriveMode != DriveMode.CHARACTERIZATION && !modulesOrienting) {
+      // Run robot at desiredSpeeds
+      // Generate feasible next setpoint
+      currentSetpoint =
+          setpointGenerator.generateSetpoint(
+              currentModuleLimits, currentSetpoint, desiredSpeeds, Constants.loopPeriodSecs);
       SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
       SwerveModuleState[] optimizedSetpointTorques = new SwerveModuleState[4];
+
       for (int i = 0; i < modules.length; i++) {
         // Optimize setpoints
         optimizedSetpointStates[i] =
@@ -401,6 +433,14 @@ public double[] getWheelRadiusCharacterizationPosition() {
     return Arrays.stream(modules).mapToDouble(Module::getPositionRads).toArray();
   }
 
+  /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */
+  private void setBrakeMode(boolean enabled) {
+    if (brakeModeEnabled != enabled) {
+      Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled));
+    }
+    brakeModeEnabled = enabled;
+  }
+
   /**
    * Returns command that orients all modules to {@code orientation}, ending when the modules have
    * rotated.
@@ -415,11 +455,14 @@ public Command orientModules(Rotation2d orientation) {
    */
   public Command orientModules(Rotation2d[] orientations) {
     return run(() -> {
+          SwerveModuleState[] states = new SwerveModuleState[4];
           for (int i = 0; i < orientations.length; i++) {
             modules[i].runSetpoint(
                 new SwerveModuleState(0.0, orientations[i]),
                 new SwerveModuleState(0.0, new Rotation2d()));
+            states[i] = new SwerveModuleState(0.0, modules[i].getAngle());
           }
+          currentSetpoint = new SwerveSetpoint(new ChassisSpeeds(), states);
         })
         .until(
             () ->

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java

@@ -94,6 +94,12 @@ public void runCharacterization(double turnSetpointRads, double input) {
     io.runCharacterization(input);
   }
 
+  /** Sets brake mode to {@code enabled}. */
+  public void setBrakeMode(boolean enabled) {
+    io.setDriveBrakeMode(enabled);
+    io.setTurnBrakeMode(enabled);
+  }
+
   /** Stops motors. */
   public void stop() {
     io.stop();

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java

@@ -22,6 +22,8 @@
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.RobotController;
 import java.util.Queue;
+import java.util.concurrent.Executor;
+import java.util.concurrent.Executors;
 import java.util.function.Supplier;
 
 public class ModuleIOKrakenFOC implements ModuleIO {
@@ -52,6 +54,7 @@ public class ModuleIOKrakenFOC implements ModuleIO {
   // Controller Configs
   private final TalonFXConfiguration driveTalonConfig = new TalonFXConfiguration();
   private final TalonFXConfiguration turnTalonConfig = new TalonFXConfiguration();
+  private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8);
 
   // Control
   private final VoltageOut voltageControl = new VoltageOut(0).withUpdateFreqHz(0);
@@ -227,16 +230,26 @@ public void setTurnPID(double kP, double kI, double kD) {
 
   @Override
   public void setDriveBrakeMode(boolean enable) {
-    driveTalonConfig.MotorOutput.NeutralMode =
-        enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
-    driveTalon.getConfigurator().apply(driveTalonConfig);
+    brakeModeExecutor.execute(
+        () -> {
+          synchronized (driveTalonConfig) {
+            driveTalonConfig.MotorOutput.NeutralMode =
+                enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+            driveTalon.getConfigurator().apply(driveTalonConfig, 0.25);
+          }
+        });
   }
 
   @Override
   public void setTurnBrakeMode(boolean enable) {
-    turnTalonConfig.MotorOutput.NeutralMode =
-        enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
-    turnTalon.getConfigurator().apply(turnTalonConfig);
+    brakeModeExecutor.execute(
+        () -> {
+          synchronized (turnTalonConfig) {
+            turnTalonConfig.MotorOutput.NeutralMode =
+                enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+            turnTalon.getConfigurator().apply(turnTalonConfig, 0.25);
+          }
+        });
   }
 
   @Override

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java

@@ -11,11 +11,13 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import org.littletonrobotics.frc2024.Constants;
+import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.ModuleConfig;
 
 public class ModuleIOSim implements ModuleIO {
   private final DCMotorSim driveSim =
@@ -32,6 +34,9 @@ public class ModuleIOSim implements ModuleIO {
   private double turnAppliedVolts = 0.0;
   private final Rotation2d turnAbsoluteInitPosition;
 
+  private boolean driveCoast = false;
+  private SlewRateLimiter driveVoltsLimiter = new SlewRateLimiter(4.0);
+
   public ModuleIOSim(ModuleConfig config) {
     turnAbsoluteInitPosition = config.absoluteEncoderOffset();
     turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
@@ -43,6 +48,12 @@ public void updateInputs(ModuleIOInputs inputs) {
       stop();
     }
 
+    if (driveCoast && DriverStation.isDisabled()) {
+      runDriveVolts(driveVoltsLimiter.calculate(driveAppliedVolts));
+    } else {
+      driveVoltsLimiter.reset(driveAppliedVolts);
+    }
+
     driveSim.update(Constants.loopPeriodSecs);
     turnSim.update(Constants.loopPeriodSecs);
 
@@ -101,6 +112,11 @@ public void setTurnPID(double kP, double kI, double kD) {
     turnFeedback.setPID(kP, kI, kD);
   }
 
+  @Override
+  public void setDriveBrakeMode(boolean enable) {
+    driveCoast = !enable;
+  }
+
   @Override
   public void stop() {
     runDriveVolts(0.0);

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java

@@ -13,6 +13,7 @@
 import java.io.*;
 import java.math.RoundingMode;
 import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
 import java.nio.file.Path;
 import java.text.DecimalFormat;
 import java.util.HashMap;
@@ -47,6 +48,24 @@ public static void main(String[] args) {
                     * 0.75)
             .build();
 
+    // Delete old trajectories
+    try {
+      Files.list(Path.of("src", "main", "deploy", "trajectories"))
+          .forEach(
+              (path) -> {
+                String filename = path.getFileName().toString();
+                if (!filename.endsWith(".pathblob")) return;
+                String[] components = filename.split("\\.");
+                if (components.length == 2
+                    && !DriveTrajectories.paths.keySet().contains(components[0])) {
+                  path.toFile().delete();
+                  System.out.println(components[0] + " - Deleted 💀");
+                }
+              });
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+
     // Check hashcodes
     Map<String, List<PathSegment>> pathQueue = new HashMap<>();
     for (Map.Entry<String, List<PathSegment>> entry : DriveTrajectories.paths.entrySet()) {