Add physics fuel sim

comp-bot/src/main/java/frc/robot/Robot.java

@@ -1,18 +1,29 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Meter;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Radian;
+
 import com.team581.Base581Robot;
 import com.team581.GlobalConfig;
 import com.team581.controller.ControllerBindings;
+import com.team581.math.MathHelpers;
 import com.team581.math.PoseErrorTolerance;
+import com.team581.simkit.FuelSim;
 import com.team581.trailblazer.Trailblazer;
 import com.team581.trailblazer.followers.PidPathFollower;
 import com.team581.trailblazer.trackers.HeuristicPathTracker;
 import com.team581.util.FieldUtil;
 import com.team581.util.FmsUtil;
 import dev.doglog.DogLog;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.autos.Autos;
+import frc.robot.autos.BumpCrossingFollower;
 import frc.robot.cluster_map.ClusterMap;
 import frc.robot.config.FeatureFlags;
 import frc.robot.conveyor.Conveyor;
@@ -41,16 +52,6 @@
 public class Robot extends Base581Robot {
   private final Hardware hardware = new Hardware();
 
-  private final Trailblazer trailblazer =
-      new Trailblazer(
-          new HeuristicPathTracker(new PoseErrorTolerance(0.5, 10)),
-          new PidPathFollower(
-              new PIDController(3.5, 0, 0),
-              new PIDController(
-                  Swerve.ORIGINAL_HEADING_PID.getP(),
-                  Swerve.ORIGINAL_HEADING_PID.getI(),
-                  Swerve.ORIGINAL_HEADING_PID.getD())));
-
   private final Limelight shooterLimelight =
       new Limelight("shooter", LimelightState.TAGS, CameraConfigs.SHOOTER);
   private final Limelight leftLimelight =
@@ -61,9 +62,25 @@ public class Robot extends Base581Robot {
       new Limelight("ground", LimelightState.CLUSTER_MAP, CameraConfigs.GROUND);
   private final HealthManager health =
       new HealthManager(shooterLimelight, leftLimelight, rightLimelight, groundLimelight);
+
+  private final Imu imu = new Imu(hardware.drivetrain);
+
+  private final Trailblazer trailblazer =
+      new Trailblazer(
+          new HeuristicPathTracker(new PoseErrorTolerance(0.5, 10)),
+          new BumpCrossingFollower(
+              new PidPathFollower(
+                  new PIDController(3.5, 0, 0),
+                  new PIDController(
+                      Swerve.ORIGINAL_HEADING_PID.getP(),
+                      Swerve.ORIGINAL_HEADING_PID.getI(),
+                      Swerve.ORIGINAL_HEADING_PID.getD())),
+              imu.bumpCrossingTracker));
+
   private final Swerve swerve =
       new Swerve(hardware.drivetrain, health, hardware.driverController, trailblazer);
-  private final Imu imu = new Imu(swerve.drivetrain);
+
+  public FuelSim fuelSim = new FuelSim("FuelSim");
 
   private final ShooterHood shooterHood = new ShooterHood(hardware.shooterHoodMotor);
 
@@ -138,6 +155,12 @@ public Robot() {
         throw new RuntimeException("Failed to write field obstacles SVG", e);
       }
     }
+
+    if (RobotBase.isSimulation()) {
+      initFuelSim();
+    }
+
+    FieldUtil.debugLogFieldZones();
   }
 
   @Override
@@ -151,6 +174,58 @@ public void robotPeriodic() {
     }
   }
 
+  @Override
+  public void simulationPeriodic() {
+    if (hopperManager.isShooting()) {
+      fuelSim.launchFuel(
+          LinearVelocity.ofBaseUnits(
+              MathHelpers.rpmToLinearVelocity(shooter.getAverageRPM(), Units.inchesToMeters(1.6)),
+              MetersPerSecond),
+          Angle.ofBaseUnits(Math.toRadians(90 - shooterHood.getAngle()), Radian),
+          Angle.ofBaseUnits(Math.PI, Radian),
+          Distance.ofBaseUnits(
+              Units.inchesToMeters(20),
+              Meter)); // Spawns a fuel onto the field at the robot's position with a specified
+      // launch velocity and angles, accounting for robot movement (robot must be
+      // registered)
+    }
+    fuelSim.updateSim();
+  }
+
+  private void initFuelSim() {
+    fuelSim.setMaxAdditions(100);
+
+    // Register a robot for collision with fuel
+    fuelSim.registerRobot(
+        Units.inchesToMeters(34.5), // from left to right in meters
+        Units.inchesToMeters(33), // from front to back in meters
+        Units.inchesToMeters(4.5), // from floor to top of bumpers in meters
+        Units.inchesToMeters(21),
+        Units.inchesToMeters(-12),
+        Units.inchesToMeters(24.5),
+        () -> localization.getPose(), // Supplier<Pose2d> of robot pose
+        () -> swerve.getFieldRelativeSpeeds()); // Supplier<ChassisSpeeds> of field-centric chassis
+    // speeds
+
+    // Register an intake to remove fuel from the field as a rectangular bounding box
+    fuelSim.registerIntake(
+        Units.inchesToMeters(16.5),
+        Units.inchesToMeters(25),
+        -Units.inchesToMeters(17.25),
+        Units.inchesToMeters(17.25), // robot-centric coordinates for bounding box in meters
+        () -> {
+          return hopperManager.isIntaking();
+        }); // (optional) Runnable called whenever a fuel is intaked
+
+    fuelSim.setSubticks(
+        5); // sets the number of physics iterations to perform per 20ms loop. Default = 5
+
+    fuelSim.start(); // enables the simulation to run (updateSim must still be called periodically)
+
+    fuelSim.enableAirResistance(); // an additional drag force will be applied to fuel in physics
+    // update step
+  }
+
   @Override
   protected void configureBindings() {
     var driver =

comp-bot/src/main/java/frc/robot/autos/BumpCrossingFollower.java

@@ -0,0 +1,72 @@
+package frc.robot.autos;
+
+import com.team581.mechanisms.imu.BumpCrossingState;
+import com.team581.mechanisms.imu.BumpCrossingTracker;
+import com.team581.trailblazer.AutoPoint;
+import com.team581.trailblazer.followers.PathFollower;
+import com.team581.trailblazer.segments.AutoSegment;
+import com.team581.util.FmsUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/**
+ * Wraps a {@link PathFollower} but injects custom velocities to ensure consistent bump crossing
+ * behavior.
+ */
+public class BumpCrossingFollower implements PathFollower {
+  private static final double CROSSING_UPHILL_LINEAR_VELOCITY = 3;
+  private static final double CROSSING_DOWNHILL_LINEAR_VELOCITY = 1;
+
+  private final PathFollower follower;
+
+  // TODO: Actually use this
+  @SuppressWarnings("unused")
+  private final BumpCrossingTracker bumpCrossingTracker;
+
+  public BumpCrossingFollower(PathFollower follower, BumpCrossingTracker bumpCrossingTracker) {
+    this.follower = follower;
+    this.bumpCrossingTracker = bumpCrossingTracker;
+  }
+
+  @Override
+  public ChassisSpeeds calculateSpeeds(
+      ChassisSpeeds currentSpeeds,
+      Pose2d currentPose,
+      Pose2d targetPose,
+      AutoPoint<?> currentPoint,
+      AutoSegment segment,
+      int currentPointIndex) {
+    var originalSpeeds =
+        follower.calculateSpeeds(
+            currentSpeeds, currentPose, targetPose, currentPoint, segment, currentPointIndex);
+
+    var state = resolveState();
+
+    if (state == BumpCrossingState.NOT_ON_BUMP) {
+      return originalSpeeds;
+    }
+
+    var unsignedXVelocity =
+        state == BumpCrossingState.CROSSING_UPHILL
+            ? CROSSING_UPHILL_LINEAR_VELOCITY
+            : CROSSING_DOWNHILL_LINEAR_VELOCITY;
+
+    var xVelocity = FmsUtil.isRedAlliance() ? unsignedXVelocity : -unsignedXVelocity;
+
+    // We preserve the Y and angular speeds to ensure we stay centered with the bump and maintain
+    // the right heading
+    return new ChassisSpeeds(
+        xVelocity, originalSpeeds.vyMetersPerSecond, originalSpeeds.omegaRadiansPerSecond);
+  }
+
+  @Override
+  public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
+    follower.reset(currentPose, currentSpeeds);
+  }
+
+  // TODO: We need to decide what state of crossing the bump (if any) we're in via
+  // BumpCrossingTracker
+  private BumpCrossingState resolveState() {
+    return BumpCrossingState.NOT_ON_BUMP;
+  }
+}

comp-bot/src/main/java/frc/robot/cluster_map/ClusterMap.java

@@ -1,5 +1,8 @@
 package frc.robot.cluster_map;
 
+import static com.google.common.collect.ImmutableMap.toImmutableMap;
+import static java.util.Map.Entry.comparingByValue;
+
 import com.team581.GlobalConfig;
 import com.team581.math.GamePieceDetectionCalculator;
 import com.team581.math.MathHelpers;
@@ -25,6 +28,7 @@
 import frc.robot.vision.limelight.LimelightState;
 import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.Map;
 import java.util.Optional;
 
 public class ClusterMap extends StateMachineSubsystem<ClusterMapState> {
@@ -94,40 +98,27 @@ public ClusterMap(Localization localization, Swerve swerve, Limelight limelight)
     this.limelight = limelight;
   }
 
+  /** Returns the lane with the most detected balls, ignoring the trench. */
   public Lane getBestClusterLane() {
-    if (clusterMap.isEmpty()) {
-      return Lane.NONE;
-    }
-
     var robotPose = localization.getPose();
 
-    int[] ballsPerLane = new int[Lane.values().length];
-
-    for (ClusterMapElement element : clusterMap) {
-      Pose2d elementPose = new Pose2d(element.clusterTranslation(), Rotation2d.kZero);
-      Lane lane = laneSystem.getLane(elementPose, robotPose);
-
-      if (lane != Lane.NONE && lane != Lane.TRENCH) {
-        ballsPerLane[lane.ordinal()] =
-            (int) (ballsPerLane[lane.ordinal()] + element.detectionSize());
-      }
-    }
-
-    Lane bestLane = Lane.NONE;
-    int maxBalls = 0;
-
-    // Find the lane with the highest count
-    for (Lane lane : Lane.values()) {
-      if (lane == Lane.NONE || lane == Lane.TRENCH) continue;
-
-      int count = ballsPerLane[lane.ordinal()];
-      if (count > maxBalls) {
-        maxBalls = count;
-        bestLane = lane;
-      }
-    }
-
-    return bestLane;
+    return clusterMap.stream()
+        // Sum up total detected balls per lane
+        .collect(
+            toImmutableMap(
+                element ->
+                    laneSystem.getLane(
+                        new Pose2d(element.clusterTranslation(), Rotation2d.kZero), robotPose),
+                element -> (int) element.detectionSize(),
+                Integer::sum))
+        .entrySet()
+        .stream()
+        // Only consider actual scoring lanes
+        .filter(entry -> entry.getKey() != Lane.NONE && entry.getKey() != Lane.TRENCH)
+        // Pick the lane with the highest ball count
+        .max(comparingByValue())
+        .map(Map.Entry::getKey)
+        .orElse(Lane.NONE);
   }
 
   public Optional<Pose2d> getBestClusterPose() {

comp-bot/src/main/java/frc/robot/config/RobotKind.java

@@ -3,13 +3,15 @@
 import edu.wpi.first.wpilibj.RobotController;
 
 public enum RobotKind {
+  PRACTICE_BOT,
   COMP_BOT;
 
   public static final boolean IS_COMP_BOT = fromSerialNumber() == COMP_BOT;
 
   /** Returns the RobotKind by matching the serial number to a known RobotKind. */
   public static RobotKind fromSerialNumber() {
     return switch (RobotController.getSerialNumber()) {
+      case "0322443D" -> PRACTICE_BOT;
       default -> COMP_BOT;
     };
   }

comp-bot/src/main/java/frc/robot/deploy/Deploy.java

@@ -136,7 +136,7 @@ protected void afterTransition(DeployState newState) {
           differentialMechanism.setControl(
               voltageRequest.withOutput(DeployConfig.HOMING_VOLTAGE_OUTWARD),
               voltageDifferentialRequest.withOutput(0));
-      case HOPPER_COMPACTION_IN, HOPPER_COMPACTION_WAITING -> {
+      case SCORE_COMPACTION, SCORE_COMPACTION_WAITING -> {
         differentialMechanism.setControl(
             motionMagicTorqueCurrentFOCAverageRequest.withPosition(clamp(newState.getLength())),
             positionDifferentialRequest.withPosition(0.0));
@@ -167,14 +167,21 @@ public boolean atGoal() {
   public void waitHopperCompactionRequest() {
     switch (getState()) {
       case UNHOMED, HOME_INWARD, HOME_OUTWARD -> {}
-      default -> setStateFromRequest(DeployState.HOPPER_COMPACTION_WAITING);
+      default -> setStateFromRequest(DeployState.SCORE_COMPACTION_WAITING);
+    }
+  }
+
+  public void feedCompactionRequest() {
+    switch (getState()) {
+      case UNHOMED, HOME_INWARD, HOME_OUTWARD -> {}
+      default -> setStateFromRequest(DeployState.FEED_COMPACTION);
     }
   }
 
   public void hopperCompactionRequest() {
     switch (getState()) {
       case UNHOMED, HOME_INWARD, HOME_OUTWARD -> {}
-      default -> setStateFromRequest(DeployState.HOPPER_COMPACTION_IN);
+      default -> setStateFromRequest(DeployState.SCORE_COMPACTION);
     }
   }
 

comp-bot/src/main/java/frc/robot/deploy/DeployConfig.java

@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.configs.TorqueCurrentConfigs;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import frc.robot.config.RobotKind;
 
 public class DeployConfig {
   public static final double MAX_LENGTH = 11.8;
@@ -23,7 +24,7 @@ public class DeployConfig {
 
   private static final Slot0Configs AVERAGE_GAINS =
       new Slot0Configs()
-          .withKP(8.0)
+          .withKP(RobotKind.IS_COMP_BOT ? 0 : 8.0)
           .withKI(0)
           .withKD(0.0)
           .withKG(0.0)

comp-bot/src/main/java/frc/robot/deploy/DeployState.java

@@ -7,8 +7,10 @@ public enum DeployState {
   INTAKE(DeployConfig.MAX_LENGTH),
   STOW(1.0),
   // placeholder values
-  HOPPER_COMPACTION_WAITING(DeployConfig.MAX_LENGTH - 3.5),
-  HOPPER_COMPACTION_IN(1.0),
+  SCORE_COMPACTION_WAITING(DeployConfig.MAX_LENGTH - 3.5),
+  SCORE_COMPACTION(1.0),
+  FEED_COMPACTION(5.915),
+
   SUPER_COMPACTION_IN(1.0),
 
   HOME_INWARD(DeployConfig.MIN_LENGTH),