Sotm with newtons law

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

@@ -70,6 +70,7 @@
 import frc.robot.utils.FieldUtils.ClimbTargets;
 import frc.robot.utils.FieldUtils.FeedTargets;
 import frc.robot.utils.FieldUtils.TrenchPoses;
+import frc.robot.utils.FuelSim;
 import frc.robot.utils.autoaim.AutoAim;
 import java.io.File;
 import java.util.Arrays;
@@ -207,6 +208,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;
@@ -419,7 +422,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 =
@@ -588,6 +592,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 */
@@ -697,6 +726,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
     //     .and(
     new Trigger(AutoAim::targetInTurretDeadzone)
         .and(() -> Superstructure.getState().isAScoreState())
+        .and(() -> !Superstructure.getState().isAFlowState())
         .and(() -> !Superstructure.getPoseOverride())
         .and(() -> superstructure.inScoringArea())
         .whileTrue(
@@ -713,6 +743,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
 
     new Trigger(AutoAim::targetInTurretDeadzone)
         .and(() -> Superstructure.getState().isAFeedState())
+        .and(() -> !Superstructure.getState().isAFlowState())
         .and(() -> !Superstructure.getPoseOverride())
         .and(() -> !superstructure.inScoringArea())
         .whileTrue(
@@ -919,6 +950,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",

src/main/java/frc/robot/Superstructure.java

@@ -68,6 +68,21 @@ public boolean isAScoreState() {
     public boolean isAFeedState() {
       return this == FEED || this == SPIN_UP_FEED || this == SPIN_UP_FEED_FLOW || this == FEED_FLOW;
     }
+
+    public boolean isAnIntakeState() {
+      return this == INTAKE
+          || this == SPIN_UP_FEED_FLOW
+          || this == FEED_FLOW
+          || this == SPIN_UP_SCORE_FLOW
+          || this == SCORE_FLOW;
+    }
+
+    public boolean isAFlowState() {
+      return this == FEED_FLOW
+          || this == SCORE_FLOW
+          || this == SPIN_UP_FEED_FLOW
+          || this == SPIN_UP_SCORE_FLOW;
+    }
   }
 
   public enum ShotTarget {
@@ -293,7 +308,8 @@ private void addTriggers() {
         new Trigger(shooter::atFlywheelVelocitySetpoint)
             // .debounce(0.05)
             .and(new Trigger(shooter::atHoodSetpoint).debounce(0.05))
-            .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05));
+            .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05))
+            .and(new Trigger(AutoAim::targetInTurretDeadzone).negate());
   }
 
   private void addTransitions() {
@@ -325,6 +341,11 @@ private void addTransitions() {
 
       bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.SCORE_FLOW, readyTrigger);
 
+      bindTransition(
+          SuperState.SCORE_FLOW,
+          SuperState.SPIN_UP_SCORE_FLOW,
+          new Trigger(AutoAim::targetInTurretDeadzone));
+
       bindTransition(
           SuperState.SPIN_UP_SCORE_FLOW,
           SuperState.IDLE,
@@ -360,6 +381,11 @@ private void addTransitions() {
 
       bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.FEED_FLOW, readyTrigger);
 
+      bindTransition(
+          SuperState.FEED_FLOW,
+          SuperState.SPIN_UP_FEED_FLOW,
+          new Trigger(AutoAim::targetInTurretDeadzone));
+
       bindTransition(
           SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
 
@@ -455,7 +481,7 @@ private void addCommands() {
         shooter.feed(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FeedTargets.getFeedTarget(feedTarget).getTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -469,7 +495,7 @@ private void addCommands() {
         intake.agitate(),
         indexer.kick(
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                         shooter.getTurretPose(swerve.getPose()),
                         FeedTargets.getFeedTarget(feedTarget).getTranslation(),
                         swerve.getVelocityFieldRelative(),
@@ -478,7 +504,7 @@ private void addCommands() {
         shooter.feed(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FeedTargets.getFeedTarget(feedTarget).getTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -494,7 +520,7 @@ private void addCommands() {
         shooter.feed(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FeedTargets.getFeedTarget(feedTarget).getTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -508,7 +534,7 @@ private void addCommands() {
         intake.intake(),
         indexer.kick(
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                         shooter.getTurretPose(swerve.getPose()),
                         FeedTargets.getFeedTarget(feedTarget).getTranslation(),
                         swerve.getVelocityFieldRelative(),
@@ -517,7 +543,7 @@ private void addCommands() {
         shooter.feed(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FeedTargets.getFeedTarget(feedTarget).getTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -533,7 +559,7 @@ private void addCommands() {
         shooter.score(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FieldUtils.getCurrentHubTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -549,7 +575,7 @@ private void addCommands() {
         // intake.restExtended(),
         indexer.kick(
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                         shooter.getTurretPose(swerve.getPose()),
                         FieldUtils.getCurrentHubTranslation(),
                         swerve.getVelocityFieldRelative(),
@@ -560,7 +586,7 @@ private void addCommands() {
         shooter.score(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FieldUtils.getCurrentHubTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -572,12 +598,12 @@ private void addCommands() {
 
     bindCommands(
         SuperState.SPIN_UP_SCORE_FLOW,
-        intake.restExtended(),
+        intake.intake(),
         indexer.rest(),
         shooter.score(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FieldUtils.getCurrentHubTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -592,7 +618,7 @@ private void addCommands() {
         intake.intake(),
         indexer.kick(
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                         shooter.getTurretPose(swerve.getPose()),
                         FieldUtils.getCurrentHubTranslation(),
                         swerve.getVelocityFieldRelative(),
@@ -603,7 +629,7 @@ private void addCommands() {
         shooter.score(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FieldUtils.getCurrentHubTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -653,7 +679,7 @@ private void addCommands() {
         shooter.score(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FieldUtils.getCurrentHubTranslation(),
                     swerve.getVelocityFieldRelative(),
@@ -668,7 +694,7 @@ private void addCommands() {
         intake.restRetracted(),
         indexer.kick(
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                         shooter.getTurretPose(swerve.getPose()),
                         FieldUtils.getCurrentHubTranslation(),
                         swerve.getVelocityFieldRelative(),
@@ -679,7 +705,7 @@ private void addCommands() {
         shooter.score(
             swerve::getPose,
             () ->
-                AutoAim.getCompensatedSOTMShotData(
+                AutoAim.getSOTMShotDataNewtonsMethod(
                     shooter.getTurretPose(swerve.getPose()),
                     FieldUtils.getCurrentHubTranslation(),
                     swerve.getVelocityFieldRelative(),

src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java

@@ -4,6 +4,12 @@
 
 package frc.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.GravityTypeValue;
@@ -17,15 +23,20 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Superstructure;
 import frc.robot.components.cancoder.CANcoderIO;
 import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged;
 import frc.robot.utils.FieldUtils;
+import frc.robot.utils.FuelSim;
 import frc.robot.utils.autoaim.AutoAim;
 import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData;
 import java.util.function.BooleanSupplier;
@@ -46,7 +57,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter {
   public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0;
   public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find
 
-  public static final double FLYWHEEL_DIAMETER_INCHES = 4;
+  public static final double FLYWHEEL_DIAMETER_INCHES = 4.0;
 
   public static final Rotation2d TURRET_LEFT_FIXED_SHOT_ANGLE = Rotation2d.kZero;
   public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero;
@@ -113,17 +124,21 @@ public class TurretSubsystem extends SubsystemBase implements Shooter {
           "Turret may have gone past hardstop!! Reoffset cancoders + min/max position",
           AlertType.kError);
 
+  private FuelSim fuelSim;
+
   public TurretSubsystem(
       FlywheelIO flywheelIO,
       HoodIO hoodIO,
       TurretIO turretIO,
       CANcoderIO cancoder24t,
-      CANcoderIO cancoder26t) {
+      CANcoderIO cancoder26t,
+      FuelSim fuelSim) {
     this.flywheelIO = flywheelIO;
     this.hoodIO = hoodIO;
     this.turretIO = turretIO;
     this.cancoder24t = cancoder24t;
     this.cancoder26t = cancoder26t;
+    this.fuelSim = fuelSim;
 
     // assume we start up at min angle and not 0
     hoodIO.resetEncoder(HOOD_MIN_ANGLE);
@@ -175,8 +190,30 @@ public void periodic() {
     //             && (getCalculatedTurretRotations().getDegrees()
     //                 < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees()));
     // if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert
+
+    if (Superstructure.getState().isAScoreState()) {
+      System.out.println("launching fuel");
+      fuelSim.launchFuel(
+          // there are few things i despise more than the units library\
+          // InchesPerSecond.of(
+          //     flywheelIO.getSetpointRotPerSec() * FLYWHEEL_DIAMETER_INCHES * Math.PI),
+          // InchesPerSecond.of(200),
+          angularToLinearVelocity(
+              RotationsPerSecond.of(flywheelIO.getSetpointRotPerSec()),
+              Inches.of(FLYWHEEL_DIAMETER_INCHES / 2)),
+          Rotation2d.fromDegrees(90).minus(hoodIO.getHoodSetpoint()).getMeasure(),
+          turretIO.getTurretSetpoint().getMeasure(),
+          Inches.of(13.75));
+    }
   }
 
+  public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) {
+    return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters) * 0.54 + 1);
+  }
+
+  @Override
+  public void simulationPeriodic() {}
+
   @Override
   public Command feed(
       Supplier<Pose2d> robotPoseSupplier,