Merge remote-tracking branch 'origin/main' into 227-orient-modules-to…

…-start-of-trajectory

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

@@ -312,6 +312,7 @@ public RobotContainer() {
                 && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED
                 && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_CLIMB
                 && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB
+                && superstructure.getCurrentGoal() != Superstructure.Goal.POST_PREPARE_TRAP_CLIMB
                 && superstructure.getCurrentGoal() != Superstructure.Goal.CLIMB
                 && superstructure.getCurrentGoal() != Superstructure.Goal.TRAP
                 && superstructure.getCurrentGoal() != Superstructure.Goal.UNTRAP);
@@ -524,6 +525,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 start of {@link HolonomicTrajectory}, ending when
    * the modules have rotated.
@@ -433,12 +473,15 @@ 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(
                 SwerveModuleState.optimize(
                     new SwerveModuleState(0.0, orientations[i]), modules[i].getAngle()),
                 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/leds/Leds.java

@@ -170,7 +170,7 @@ public synchronized void periodic() {
 
       // Same battery alert
       if (sameBattery) {
-        strobe(Color.kRed, strobeFastDuration);
+        breath(Color.kRed, Color.kBlack);
       }
     } else if (DriverStation.isAutonomous()) {
       wave(Color.kGold, Color.kDarkBlue, waveFastCycleLength, waveFastDuration);

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/RollersSensorsIOCompbot.java

@@ -17,7 +17,7 @@ public class RollersSensorsIOCompbot implements RollersSensorsIO {
   private final DigitalGlitchFilter glitchFilter = new DigitalGlitchFilter();
 
   public RollersSensorsIOCompbot() {
-    glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(5).toNanos());
+    glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos());
     glitchFilter.add(shooterStagedSensor);
     glitchFilter.add(backpackStagedSensor);
   }

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java

@@ -35,6 +35,7 @@ public enum Goal {
     RESET_CLIMB,
     PREPARE_PREPARE_TRAP_CLIMB,
     PREPARE_CLIMB,
+    POST_PREPARE_TRAP_CLIMB,
     CLIMB,
     TRAP,
     UNTRAP,
@@ -72,6 +73,7 @@ public void periodic() {
     if (!climber.retracted()
         && desiredGoal != Goal.PREPARE_PREPARE_TRAP_CLIMB
         && desiredGoal != Goal.PREPARE_CLIMB
+        && desiredGoal != Goal.POST_PREPARE_TRAP_CLIMB
         && desiredGoal != Goal.CLIMB
         && desiredGoal != Goal.TRAP
         && desiredGoal != Goal.UNTRAP
@@ -159,6 +161,11 @@ public void periodic() {
         climber.setGoal(Climber.Goal.EXTEND);
         backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
       }
+      case POST_PREPARE_TRAP_CLIMB -> {
+        arm.setGoal(Arm.Goal.CLIMB);
+        climber.setGoal(Climber.Goal.EXTEND);
+        backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
+      }
       case CLIMB -> {
         arm.setGoal(Arm.Goal.CLIMB);
         climber.setGoal(Climber.Goal.RETRACT);

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java

@@ -46,7 +46,8 @@ public class ArmConstants {
   /** The offset of the arm encoder in radians. */
   public static final double armEncoderOffsetRads =
       switch (Constants.getRobot()) {
-        default -> 1.1980389953386859; // 1.0753205323078345 rad as measured at hardstop on 3/12/24,
+        default ->
+            1.1980389953386859 + .006; // 1.0753205323078345 rad as measured at hardstop on 3/12/24,
           // corresponding to an arm position of 0.11965050145508001 rad
         case DEVBOT -> -1.233 - Math.PI / 2.0;
       };

trajectory_native/src/trajectory_service.cpp

@@ -19,7 +19,7 @@ namespace vts = org::littletonrobotics::vehicletrajectoryservice;
 // We take the tradeoff of increased computation time since we cache paths anyway.
 static const double CONTROL_INTERVAL_GUESS_SCALAR = 1.0;
 
-static const int MINIMUM_CONTROL_INTERVAL_COUNT = 32;
+static const int MINIMUM_CONTROL_INTERVAL_COUNT = 20;
 
 static std::unique_ptr<grpc::Server> server;