Yet another sotm rewrite

src/main/java/frc/robot/Robot.java

@@ -71,7 +71,8 @@
 import frc.robot.utils.FieldUtils.ClimbTargets;
 import frc.robot.utils.FieldUtils.FeedTargets;
 import frc.robot.utils.FieldUtils.TrenchPoses;
-import frc.robot.utils.autoaim.AutoAim;
+import frc.robot.utils.FuelSim;
+import frc.robot.utils.autoaim.NewAutoAim;
 import java.io.File;
 import java.util.Arrays;
 import java.util.Optional;
@@ -208,6 +209,8 @@ public enum RobotEdition {
   private final CANdleSubsystem candle =
       new CANdleSubsystem(new CANdleIOReal(0, CANdleSubsystem.getCandleConfig(), canivore));
 
+  private FuelSim fuelSim = new FuelSim();
+
   // climber only exists for the comp bot - this is accounted for later
 
   private final Superstructure superstructure;
@@ -420,7 +423,8 @@ public Robot() {
                     : new CANcoderIOSim(5, TurretSubsystem.getCancoder24tConfigs(), canivore),
                 ROBOT_MODE == RobotMode.REAL
                     ? new CANcoderIO(4, TurretSubsystem.getCancoder26tConfigs(), canivore)
-                    : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore));
+                    : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore),
+                fuelSim);
         break;
     }
     climber =
@@ -516,7 +520,7 @@ public Robot() {
     shooter.setDefaultCommand(
         shooter.rest(
             swerve::getPose,
-            swerve::getVelocityFieldRelative,
+            swerve::getVelocityRobotRelative,
             superstructure::inScoringArea,
             () -> FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose()));
     swerve.setDefaultCommand(
@@ -589,6 +593,31 @@ public Robot() {
                   "Interrputing: "
                       + (interrupting.isPresent() ? interrupting.get().getName() : "none"));
             });
+
+    // fuelSim.spawnStartingFuel();
+
+    fuelSim.registerRobot(
+        Units.inchesToMeters(28), // from left to right in meters
+        Units.inchesToMeters(28), // from front to back in meters
+        Units.inchesToMeters(4), // from floor to top of bumpers in meters
+        swerve::getPose, // Supplier<Pose2d> of robot pose
+        swerve
+            ::getVelocityFieldRelative); // Supplier<ChassisSpeeds> of field-centric chassis speeds
+
+    fuelSim.registerIntake(
+        Units.inchesToMeters(-14),
+        Units.inchesToMeters(14),
+        Units.inchesToMeters(14),
+        Units.inchesToMeters(20), // robot-centric coordinates for bounding box in meters
+        () ->
+            Superstructure.getState()
+                .isAnIntakeState() // (optional) BooleanSupplier for whether the intake should be
+        // active at a given moment
+        ); // (optional) Runnable called whenever a fuel is intaked
+
+    fuelSim.setSubticks(5);
+
+    fuelSim.start();
   }
 
   /** Scales a joystick value for teleop driving */
@@ -651,7 +680,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                     Commands.parallel(
                         shooter.runHoodCurrentZeroing(), intake.runCurrentZeroing())));
 
-    new Trigger(() -> AutoAim.targetInTurretDeadzone())
+    new Trigger(() -> NewAutoAim.targetInTurretDeadzone())
         .onTrue(
             driver
                 .rumbleCmd(1, 1)
@@ -696,8 +725,9 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
     // driver
     //     .leftBumper()
     //     .and(
-    new Trigger(AutoAim::targetInTurretDeadzone)
+    new Trigger(NewAutoAim::targetInTurretDeadzone)
         .and(() -> Superstructure.getState().isAScoreState())
+        .and(() -> !Superstructure.getState().isAFlowState())
         .and(() -> !Superstructure.getPoseOverride())
         .and(() -> superstructure.inScoringArea())
         .whileTrue(
@@ -712,8 +742,9 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                         * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
                 shooter::getTurretPosition));
 
-    new Trigger(AutoAim::targetInTurretDeadzone)
+    new Trigger(NewAutoAim::targetInTurretDeadzone)
         .and(() -> Superstructure.getState().isAFeedState())
+        .and(() -> !Superstructure.getState().isAFlowState())
         .and(() -> !Superstructure.getPoseOverride())
         .and(() -> !superstructure.inScoringArea())
         .whileTrue(
@@ -850,7 +881,7 @@ public void robotPeriodic() {
         "trench poses",
         Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new));
 
-    Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone());
+    Logger.recordOutput("Turret/out of range", NewAutoAim.targetInTurretDeadzone());
 
     noLogStickAlert.set(!directory.exists());
 
@@ -930,6 +961,7 @@ public void simulationInit() {
 
   @Override
   public void simulationPeriodic() {
+    fuelSim.updateSim();
     // Log zeroed poses for mechs and robot for debugging in sim
     Logger.recordOutput(
         "Robot/Zeroed Mechanism Poses",
@@ -984,7 +1016,7 @@ public void teleopPeriodic() {}
   @Override
   public void teleopExit() {
     System.out.println("Saving BFG Log");
-    bfg.saveLog("");
+    if (bfg.isConnected()) bfg.saveLog("");
   }
 
   @Override