R2 102 implement static shooting calculations

src/main/java/frc/robot/Constants.java

@@ -68,6 +68,7 @@ public class FieldConstants {
      *     Contains various field dimensions and useful reference points. All units are in meters
      * and poses    have a blue alliance origin.
      */
+    // TODO: Update to 2026 Field Constants and add HUB Center
     public static final AprilTagFieldLayout aprilTagLayout =
         AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
 
@@ -121,13 +122,25 @@ public final class ShooterConstants {
     public static final Angle MAX_ANGLE = Rotations.of(10.0);
     public static final Angle STARTING_ANGLE = Rotations.of(0.0);
     public static final Distance WHEEL_RADIUS = Meters.of(0.5);
-    public static final RotaryMechCharacteristics CONSTANTS =
-        new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
+
     public static final double IDLE_SPEED_RPM = (1.0);
     public static final double HUB_SPEED_RPM = (1.0);
     public static final double TOWER_SPEED_RPM = (1.0);
     public static final double DEFAULT_SPEED_RPM = (1.0);
     public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0;
+    public static final RotaryMechCharacteristics CONSTANTS =
+        new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
+
+    // Hood Constants
+    public static final double HEIGHT_DIFFERENCE =
+        1.295; // Meters between flywheel center and top of hub opening
+    public static final double EXIT_VELOCITY = 7.4; // m/s from ReCalc Flywheel Calculator
+    public static final AngularVelocity HOOD_VELOCITY = RotationsPerSecond.of(1.0);
+    public static final AngularAcceleration HOOD_ACCELERATION = RotationsPerSecondPerSecond.of(1.0);
+    public static final Velocity<AngularAccelerationUnit> HOOD_JERK = HOOD_ACCELERATION.per(Second);
+    public static final double HOOD_TOLERANCE = 1.0; // In degrees
+    public static final double GRAVITY = 9.81; // m/s^2
+    public static final double IDLE_HOOD_ANGLE = 25.0; // degrees
   }
 
   public static final int CANDLE_ID = 50;

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

@@ -1,26 +1,36 @@
 package frc.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.W8.io.motor.MotorIO.PIDSlot;
 import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism;
+import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
 import frc.robot.Constants.FeederConstants;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.ShooterConstants;
 
 public class Shooter extends SubsystemBase {
 
-  private FlywheelMechanism _flywheel;
-  private FlywheelMechanism _feeder;
-  public double desiredVelo;
+  private final FlywheelMechanism _flywheel;
+  private final FlywheelMechanism _feeder;
+  private final RotaryMechanism _hood;
 
-  public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder) {
+  // desired target values
+  private double desiredVelo;
+  private double hoodAngle;
+
+  public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) {
     _flywheel = flywheel;
     _feeder = feeder;
+    _hood = hood;
   }
 
   // Sets feeder motor speed
@@ -31,8 +41,9 @@ public void runFeeder() {
 
   // Sets the flywheel velocity based on an input.
   public void setFlywheelVelocity(double velocity) {
+    // store the desired velocity then send converted velocity to the mechanism
+    this.desiredVelo = velocity;
     AngularVelocity angVelo = RotationsPerSecond.of(velocity);
-    velocity = desiredVelo;
 
     _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0);
   }
@@ -58,20 +69,55 @@ public boolean flyAtVelocity() {
         <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE;
   }
 
+  public double getHoodAngleDegrees(Translation2d robotPos) {
+
+    // TODO: Replace with HUB later once it gets added.
+    double distance = robotPos.getDistance(FieldConstants.FIELDCENTER);
+
+    double check =
+        Math.pow(ShooterConstants.EXIT_VELOCITY, 4)
+            - ShooterConstants.GRAVITY
+                * (ShooterConstants.GRAVITY * Math.pow(distance, 2)
+                    + 2
+                        * ShooterConstants.HEIGHT_DIFFERENCE
+                        * Math.pow(ShooterConstants.EXIT_VELOCITY, 2));
+
+    if (check < 0) {
+      return ShooterConstants.IDLE_HOOD_ANGLE; // Default angle if the shot is not possible
+    }
+    return Math.toDegrees(
+        Math.atan(
+            (ShooterConstants.EXIT_VELOCITY * ShooterConstants.EXIT_VELOCITY + Math.sqrt(check))
+                / (ShooterConstants.GRAVITY * distance)));
+  }
+
+  // Sets hood angle
+  public void setHoodAngle(double angleDegrees) {
+    hoodAngle = angleDegrees;
+    _hood.runPosition(
+        Angle.ofBaseUnits(angleDegrees, Degrees),
+        ShooterConstants.HOOD_VELOCITY,
+        ShooterConstants.HOOD_ACCELERATION,
+        ShooterConstants.HOOD_JERK,
+        PIDSlot.SLOT_0);
+  }
+
+  // Checks if hood is at angle
+  public boolean hoodAtAngle() {
+    return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE;
+  }
+
   public Command shoot(double velocity) {
-    return Commands.run(
-            () -> {
-              setFlywheelVelocity(velocity);
-            })
-        .until(() -> flyAtVelocity())
-        .andThen(
-            () -> {
-              runFeeder();
-            })
-        .andThen(
-            () -> {
-              setFlywheelVelocity(0);
-            });
+    // Prepare targets
+    return Commands.sequence(
+        // Set and wait in parallel for both hood and flywheel
+        Commands.parallel(
+            Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity),
+            Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)),
+        // feed once ready
+        Commands.runOnce(() -> runFeeder()),
+        // stop flywheel when finished
+        Commands.runOnce(() -> setFlywheelVelocity(0)));
   }
 
   @Override