Added ultrashooter command and support methods, constants, controller activation

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

@@ -14,6 +14,8 @@ import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.units.measure.Angle;
 
+import java.util.Map;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
  * numerical or boolean constants. This class should not be used for any other
@@ -141,10 +143,27 @@ public final class Constants {
         public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
         public static final double HOMING_SPEED = .05;
         public static final double TURRET_GEAR_RATIO = 6;
+        public static final double TURRET_RANGE_ROTATIONS = 0.5;            // The turret can rotate 180°
 
         public static final int HOOD_SERVO_CHANNEL = 0;
         public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
         public static final int HOOD_ENCODER_ID = 4;
+        public static final int HOOD_ANGLE_MIN_DEG = 52;                    // Lowest possible position
+        public static final int HOOD_ANGLE_MAX_DEG = 70;                    // Highest possible position
+        public static final int HOOD_ANGLE_PASSING_DEG = 55;                // Default hood angle for passing
+        public static final int HOOD_ANGLE_SCORING_MAX_DEG = 70;            // Highest angle for scoring
+        public static final int HOOD_ANGLE_SCORING_MIN_DEG = 62;            // Lowest angle for scoring
+        public static final double SCORING_DISTANCE_LONG_IN = 225;          // Shortest scoring distance in inches used for finding hood angles
+        public static final double SCORING_DISTANCE_SHORT_IN = 60;          // Shortest scoring distance in inches used for finding hood angles
+        public static final double SCORING_DISTANCE_FIELD_MAX_METERS = 7;   // Sanity check value. Max distance from hub in Alliance Zone is ~6 meters
+        public static final double SCORING_DISTANCE_FIELD_MIN_METERS = 0.1; // Sanity check value.
+        public static final double SCORING_DISTANCE_MIN_ALLOWED_METERS = 1.5;   // The minimum distance from the hub we'll allow the robot to shoot from
+        public static final double SCORING_DISTANCE_MAX_ALLOWED_METERS = 7.0;   // The maximum distance from the hub we'll allow the robot to shoot from
+        // This is the distance increment in inches for adjusting hood angle according to distance from the target, in inches per degree:
+        public static final double HOOD_ANGLE_CALC_DIVISION = (SCORING_DISTANCE_LONG_IN - SCORING_DISTANCE_SHORT_IN) / (HOOD_ANGLE_SCORING_MAX_DEG - HOOD_ANGLE_SCORING_MIN_DEG);
+        public static final double SHOOTER_EFFICIENCY_GENERAL = 0.30;       // This is the baseline shooter efficiency assumption
+        public static final double SHOOTER_FLYWHEEL_DIAMETER_IN = 3.0;      // Flywheel diameter is 3.0 inches
+        public static final double SHOOTER_FLYWHEEL_GEAR_RATIO = 2.0;       // Flywheel moves at twice the RPM of the flywheel motor
         public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
         public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
         public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
@@ -153,6 +172,41 @@ public final class Constants {
         public static final double DEFLECTOR_SET_ANGLE2 = 200;
         public static final double DEFLECTOR_SET_ANGLE3 = 300;
         public static final double DEFLECTOR_SERVO_RATIO = 1.0;
+
+        // Below are sets of quadratic coefficients that are to be used with each valid hood angle, for scoring and passing.
+        // These are from Table 2-2 in the AutoShooter document for use in scoring when distance is in meters.
+        record QuadraticCoef(double a, double b, double c) {}                               // Creates a basic record type with no special methods ("{}")
+        
+        public static Map<Integer, QuadraticCoef> SCORING_SPEED_COEFS_METERS = Map.of(      // Access this map to get scoring coefficients for each hood angle
+            62, new QuadraticCoef(-0.08267, 3.41207, 12.20000),
+            63, new QuadraticCoef(-0.12400, 3.85827, 11.50000),
+            64, new QuadraticCoef(-0.09300, 3.74016, 11.70000),
+            65, new QuadraticCoef(-0.06200, 3.54331, 12.40000),
+            66, new QuadraticCoef(-0.09300, 3.89764, 12.00000),
+            67, new QuadraticCoef(-0.06200, 3.85827, 12.20000),
+            68, new QuadraticCoef(0.00000, 3.54331, 13.00000),
+            69, new QuadraticCoef(0.03100, 3.58268, 13.10000),
+            70, new QuadraticCoef(0.12056, 3.25022, 13.96667));
+
+        // These are from Table 2-2 in the AutoShooter document for use in scoring when distance is in inches.
+        public static Map<Integer, QuadraticCoef> SCORING_SPEED_COEFS_INCHES = Map.of(      // Access this map to get scoring coefficients for each hood angle
+            62, new QuadraticCoef(-0.00005, 0.08667, 12.20000),
+            63, new QuadraticCoef(-0.00008, 0.09800, 11.50000),
+            64, new QuadraticCoef(-0.00006, 0.09500, 11.70000),
+            65, new QuadraticCoef(-0.00004, 0.09000, 12.40000),
+            66, new QuadraticCoef(-0.00006, 0.09900, 12.00000),
+            67, new QuadraticCoef(-0.00004, 0.09800, 12.20000),
+            68, new QuadraticCoef(0.00000, 0.09000, 13.00000),
+            69, new QuadraticCoef(0.00002, 0.09100, 13.10000),
+            70, new QuadraticCoef(0.00008, 0.08256, 13.96667));
+        
+        // These are from Table 2-1 in the AutoShooter document for use in passing when distance is in meters.
+        public static Map<Integer, QuadraticCoef> PASSING_SPEED_COEFS_METERS = Map.of(
+            55, new QuadraticCoef(-0.09751, 3.47134, 7.8890));
+        
+        // These are from Table 2-1 in the AutoShooter document for use in passing when distance is in inches.
+        public static Map<Integer, QuadraticCoef> PASSING_SPEED_COEFS_INCHES = Map.of(
+            55, new QuadraticCoef(-0.00006, 0.08817, 7.889));
     }
 
     public static class HopperConstants {
@@ -187,7 +241,7 @@ public final class Constants {
 
     }
 
-    // These represent the dividing lines for the regions in our shooting strategy.
+    // These represent the dividing lines for the regions in our shooting strategy. These values are in meters.
     public static class ShootingRegionDimensions {
 
         public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;

src/main/java/frc/robot/RobotContainer.java

@@ -16,6 +16,7 @@ import frc.robot.commands.AutoTurretCommand;
 import frc.robot.commands.AutoTurretShootCommand;
 import frc.robot.commands.Reverse;
 import frc.robot.commands.SetDeflectorCommand;
+import frc.robot.commands.UltraShooterCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -59,7 +60,9 @@ public class RobotContainer {
         NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake(m_driverController.getHID()));
         NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
 
-        turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
+        // The default command below is commented out so that UltraShooterCommand can control turret while the shooter function is active.
+        // The default command can be re-instated if we choose to track a target at all times instead.
+        // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
     }
 
     /**
@@ -74,6 +77,7 @@ public class RobotContainer {
      *
      */
     private void configureBindings() {
+        // These let the driver set the shooter hood position using the controller.
         // m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
         // m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
         // m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
@@ -83,8 +87,10 @@ public class RobotContainer {
         m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
         m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
-        m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
+        // m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
-        m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
+        // m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
+        //m_driverController.x().toggleOnTrue(getAutonomousCommand())((new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
+        m_driverController.x().toggleOnTrue(new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
         m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
         m_driverController.y().onTrue(intakeSubsystem.deploy());
     }

src/main/java/frc/robot/Utils.java

@@ -7,9 +7,13 @@ import frc.robot.Constants.ShootingRegionDimensions;
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 
+import java.util.Map;
+
 public class Utils {
     
     /**
@@ -37,8 +41,8 @@ public class Utils {
     /**
      * Determines the region for the robot based on its location and alliance.
      * Assumes the robot is a 27" square. Thresholds based on observations from simulator.
-     * @param robotPose: the robot's pose
+     * @param robotPose the robot's pose
-     * @param alliance: the red/blue alliance of the robot, as a sanity check
+     * @param alliance the red/blue alliance of the robot, as a sanity check
      * @return the robot's shooting region
      */
     public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
@@ -46,7 +50,7 @@ public class Utils {
         double robotX = robotPose.getX();
         double robotY = robotPose.getY();
 
-        // Receommended: Add a sanity check for invalid robot poses.
+        // Recommended: Add a sanity check for invalid robot poses.
 
         if (alliance == Alliance.Blue) {
             if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
@@ -100,6 +104,39 @@ public class Utils {
         return region;
     }
 
+
+    /**
+     * Calculates the ideal hood angle based on the provided distance
+     * between the robot's turret and the target (hub).
+     * Implements the equation to find hood angle for scoring:
+     * Angle = MaxAngle - ((Distance - ShortDistance) / Div), where
+     *      MaxAngle = maximum hood angle for scoring
+     *      Distance = distance to target
+     *      ShortDistance = shortest distance used when evaluating hood angles
+     *      LongDistance = longest distance used when evaluating hood angles
+     *      Div = the distance from the target that requires 1° of hood angle change
+     *          = (LongDistance - ShortDistance) / (MaxAngle - MinAngle)
+     * These were all defined as constants in Constants.java.
+     * @param distanceInMeters distance between turret and target, in meters
+     * @return the ideal nearest-integer hood angle in degrees relative to the floor
+     */
+    public static int getScoringHoodAngleDegForDistance(double distanceInMeters){
+        double calculatedAngleDeg = Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG;      // init to the passing value
+        double distanceInInches = Units.metersToInches(distanceInMeters);                   // Convert to inches to match theory equations
+ 
+        // Limit the distance according to the min and max that were used in the theory calculations
+        MathUtil.clamp(distanceInInches, Constants.ShooterConstants.SCORING_DISTANCE_SHORT_IN, Constants.ShooterConstants.SCORING_DISTANCE_LONG_IN);
+
+        // Perfom the main calculation (Equation 2-5 in the AutoShooter document), then round the result to the nearest degree
+        calculatedAngleDeg = Constants.ShooterConstants.HOOD_ANGLE_SCORING_MAX_DEG - (((distanceInInches) - Constants.ShooterConstants.SCORING_DISTANCE_SHORT_IN) / 
+                                    Constants.ShooterConstants.HOOD_ANGLE_CALC_DIVISION);
+        int hoodAngleDeg = (int) Math.round(calculatedAngleDeg);
+
+        return (hoodAngleDeg);
+    }
+
+
+
     public static double getHoodAngleForDistance(double distanceInMeters){
         if(distanceInMeters >= 4){
             return 200;
@@ -107,6 +144,93 @@ public class Utils {
         return 0;
     }
 
+
+    /**
+     * Calculates the appropriate launch speed in ft/s for scoring based on
+     * the provided hood angle and target distance in meters.
+     * This is done based on a series of quadratic regressions
+     * See the AutoShooter theory document.
+     * @param hoodAngleDeg the hood angle that the calculation should assume
+     * @param distanceInMeters distance between turret and target, in meters
+     * @return the appropriate launch speed for scoring in feet per second
+     * Returns 0 if a launch speed cannot be determined.
+     */
+    public static double findLaunchSpeedScoring(int hoodAngleDeg, double distanceInMeters){
+        // Check that the requested hood angle is in the expected shooting range
+        if ((hoodAngleDeg < Constants.ShooterConstants.HOOD_ANGLE_SCORING_MIN_DEG) ||
+           (hoodAngleDeg > Constants.ShooterConstants.HOOD_ANGLE_SCORING_MAX_DEG)) {
+            return 0;
+        }
+
+        // Check that the requested distance is reasonable. If the scoring distance is outside of this range, there was a problem.
+        if ((distanceInMeters < Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MIN_METERS) ||
+           (distanceInMeters > Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MAX_METERS)) {
+            return 0;
+        }
+                
+        // Get the quadratic coefficients for the hood angle. Return with the error value if none are found.
+        Constants.ShooterConstants.QuadraticCoef coefficients = Constants.ShooterConstants.SCORING_SPEED_COEFS_METERS.get(hoodAngleDeg);
+        if (coefficients == null) {
+            return 0;
+        }
+        
+        // Perform the quadratic calculation using the coefficients we retrieved if successful
+        double x = distanceInMeters;                            // Rename to "x" just to make the next expression simpler
+        double launchSpeedFPS = (coefficients.a() * x * x) + (coefficients.b() * x) + coefficients.c();
+
+        return launchSpeedFPS;
+    }
+
+    /**
+     * Calculates the appropriate launch speed in ft/s for passing based on
+     * the provided hood angle and target distance in meters.
+     * This is done based on a series of quadratic regressions
+     * See the AutoShooter theory document.
+     * @param hoodAngleDeg the hood angle that the calculation should assume
+     * @param distanceInMeters distance between turret and target, in meters
+     * @return the appropriate launch speed for passing in feet per second
+     * Returns 0 if a launch speed cannot be determined.
+     */
+    public static double findLaunchSpeedPassing(int hoodAngleDeg, double distanceInMeters){
+        // Check that the requested hood angle is in the expected angle range for passing.
+        if (hoodAngleDeg != Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG) {
+            return 0;
+        }
+
+        // Check that the distance is reasonable. If the scoring distance is outside of this range, there was a problem.
+        if ((distanceInMeters < Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MIN_METERS) ||
+           (distanceInMeters > Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MAX_METERS)) {
+            return 0;
+        }
+                
+        // Get the quadratic coefficients for the hood angle. Return with the error value if none are found.
+        Constants.ShooterConstants.QuadraticCoef coefficients = Constants.ShooterConstants.PASSING_SPEED_COEFS_METERS.get(hoodAngleDeg);
+        if (coefficients == null) {
+            return 0;
+        }
+        
+        // Perform the quadratic calculation using the coefficients we retrieved if successful
+        double x = distanceInMeters;                            // Rename to "x" just to make the next expression simpler
+        double launchSpeedFPS = (coefficients.a() * x * x) + (coefficients.b() * x) + coefficients.c();
+
+        return launchSpeedFPS;
+    }
+
+    /**
+     * Calculates the flywheel RPM that's required to achieve the provided
+     * launch speed in feet/second.
+     * This performs Equation 2-3 in the AutoShooter document.
+     * @param launchSpeedFPS the goal launch speed in ft/s
+     * @return the appropriate flywheel speed in RPM
+     */
+    public static double GetShooterRPMForLaunchSpeed(double launchSpeedFPS){
+        double flywheelDiameterFeet = Constants.ShooterConstants.SHOOTER_FLYWHEEL_DIAMETER_IN / 12;         // Change to feet to match launch speed units
+        double efficiency = Constants.ShooterConstants.SHOOTER_EFFICIENCY_GENERAL;                          // Use the appropriate efficiency
+        
+        double shooterRPM = (launchSpeedFPS / (efficiency * flywheelDiameterFeet * Math.PI)) * 60;          // Perform the calculation
+        return shooterRPM;
+    }
+
     public static double getLauncherRPMForDistance(double distanceInMeters){ 
         return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
     }
@@ -117,8 +241,8 @@ public class Utils {
      * Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
      * and these exist for both red and blue alliances.
      * Right/left are from driver's perspective.
-     * @param region: the identifier of the robot's current field region
+     * @param region the identifier of the robot's current field region
-     * @param alliance: the red/blue alliance of the robot, as a sanity check
+     * @param alliance the red/blue alliance of the robot, as a sanity check
      * @return the shooting target for a robot with the provided properties. Returns all-zero
      * Pose3d object if the region/alliance combination is invalid.
      */

src/main/java/frc/robot/commands/UltraShooterCommand.java

@@ -0,0 +1,208 @@
+package frc.robot.commands;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
+import frc.robot.Constants.ShootingRegion;
+import frc.robot.Utils;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+
+public class UltraShooterCommand extends Command{
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+    private IntakeSubsystem intakeSubsystem;                // TODO: is this necessary?
+    private DriveSubsystem driveSubsystem;
+
+    private Command waitCommand = Commands.waitSeconds(1.5);
+    private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736);     // Defines physical offset of turret from robot center
+
+    private Alliance alliance;                              // Shooting behavior is based on the robot's alliance
+    
+    public UltraShooterCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+        this.driveSubsystem = driveSubsystem;
+
+        addRequirements(hopperSubsystem, intakeSubsystem);                  // TODO: isintakeSubsystem necessary?
+    }
+    
+    @Override
+    public void initialize() {
+        hopperSubsystem.spindexerOn();                                      // Activate spindexer so it's on while command is active
+        hopperSubsystem.stopKicker();                                       // Make sure kicker is off when command is called
+        CommandScheduler.getInstance().schedule(waitCommand);               // Kick off a wait timer so robot doesn't shoot until hopper is ready
+        alliance = DriverStation.getAlliance().get();                       // Fetch our alliance
+        // Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
+        Logger.recordOutput("UltraShooter-State", "ACTIVE");
+    }
+
+    @Override
+    public void execute() {
+        Boolean okToShoot = false;                      // Tracks whether conditions are OK for shooting.
+        Boolean scoring = false;                        // Are we shooting into the hub?
+        Pose3d botPose = driveSubsystem.getBotPose();   // Find the pose once since we'll use it a few times
+        
+        // STEPS 1 & 2: Get the field location of the initial target
+        // First, find the robot's current field location.
+        ShootingRegion shootingRegion = Utils.findRobotShootingRegion(botPose, alliance);
+
+        // If robot is in a valid scoring or passing region, find the field pose for the appropriate target and perform calculations
+        if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
+            
+            okToShoot = true;                                                                   // So far, so good
+            
+            // Check whether we're scoring, since it require some extra steps
+            if ((shootingRegion == ShootingRegion.BLUE_OWN_ALLIANCE_ZONE) || (shootingRegion == ShootingRegion.RED_OWN_ALLIANCE_ZONE)){
+                scoring = true;
+            }
+                        
+            Pose3d initialTargetPose = Utils.findShootingTarget(shootingRegion, alliance);      // Get initial target based on robot location
+            Logger.recordOutput("UltraShooter-InitialTarget", initialTargetPose);
+
+            // STEPS 3 & 4: calculate hood and flywheel values based on distance to the intiial target.
+            // First, find the turret's pose based on the robot's pose and the location of the turret relative to the robot's center.
+            // Note that the turret points backward, so its rotational property is +0.5 rotations.
+            Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
+                    new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
+            // Then calculate the linear distance between the initial target and the robot's turret.
+            double distanceToInitialTargetMeters = initialTargetPose.toPose2d().getTranslation().getDistance(turretPose.getTranslation().toTranslation2d());
+            Logger.recordOutput("UltraShooter-DistanceToInitialTarget(Meters)", distanceToInitialTargetMeters);
+
+            // STEP 3: Find the best hood angle
+            // Default to the passing hood angle if we're not shooting into a hub.
+            int initialHoodAngleDegrees = Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG;
+            // If we're scoring, run the calculations to find the best hood angle.
+            if (scoring) {
+                initialHoodAngleDegrees = Utils.getScoringHoodAngleDegForDistance(distanceToInitialTargetMeters);
+            }
+
+            // STEP 4: determine the right shooter flywheel speed
+            // First, find the correct launch speed in feet/second. Note the unit changes!
+            // This covers the scoring scenario then the passing scenario.
+            double intitialLaunchSpeedFPS = 0;                                                  // The target launch speed in feet per second (FPS)
+            if (scoring) {
+                intitialLaunchSpeedFPS = Utils.findLaunchSpeedScoring(initialHoodAngleDegrees, distanceToInitialTargetMeters);
+            } else {
+                intitialLaunchSpeedFPS = Utils.findLaunchSpeedPassing(initialHoodAngleDegrees, distanceToInitialTargetMeters);
+            }
+
+            // Handle error condition in the launch speed calcs
+            if (intitialLaunchSpeedFPS == 0) {
+                okToShoot = false;                
+            }
+
+            // STEP 5: Find final target based on the intial target and the robot's velocity.
+            // THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT.
+            Pose3d finalTargetPose = initialTargetPose;
+            double distanceToFinalTargetMeters = distanceToInitialTargetMeters;
+            Logger.recordOutput("UltraShooter-FinalTarget", finalTargetPose);
+            Logger.recordOutput("UltraShooter-DistanceToFinalTarget(Meters)", distanceToFinalTargetMeters);
+
+            // STEP 6: Find hood angle and launch speed for the final target.
+            // THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT:
+            double finalLaunchSpeedFPS = intitialLaunchSpeedFPS;
+            int finalHoodAngleDegrees = initialHoodAngleDegrees;
+            
+            // Confirm we're at a valid scoring distance for the robot's capabilities. If not, don't shoot.
+            if ((scoring) && 
+                ((distanceToFinalTargetMeters > Constants.ShooterConstants.SCORING_DISTANCE_MAX_ALLOWED_METERS) ||
+                (distanceToFinalTargetMeters < Constants.ShooterConstants.SCORING_DISTANCE_MIN_ALLOWED_METERS))) {
+                    okToShoot = false;
+            }
+
+            // STEP 7: Find the flywheel RPM needed to achieve the launch speed we just calculated.
+            double finalShooterRPM = Utils.GetShooterRPMForLaunchSpeed(finalLaunchSpeedFPS);
+
+            // STEP 8: Command turretSubsystem to do what we need it to do for the launch speed and launch angle.
+            // Adjust for the shooter flywheel-to-motor gear ratio when setting the flywheel motor speed.
+            turretSubsystem.setHoodToLaunchAngle(finalHoodAngleDegrees);
+            turretSubsystem.setLauncher(finalShooterRPM / Constants.ShooterConstants.SHOOTER_FLYWHEEL_GEAR_RATIO);
+
+            // STEP 9: Command the turretSubsystem to aim at the intended field target (azimuth control).
+            // First, find the x,y translation between the field positions of the robot's turret and the final target.
+            Translation2d finalTargetTranslation = finalTargetPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
+            
+            // Find the rotations value, field-relative, between turret point and target point.
+            double turretToTargetRotation = finalTargetTranslation.getAngle().getRotations();
+            
+            // Command the turret to get to this field-relative angle.
+            // Don't try to set the turret if it's outside of it's physical capability, and prevent shooting.
+            if(turretSubsystem.isTurretReady() && turretSubsystem.isTurretAnglePossible(botPose, finalTargetTranslation)) {
+                turretSubsystem.setTurretFieldRelative(botPose, turretToTargetRotation);
+            } else {
+                okToShoot = false;
+            }
+
+        } else {
+            // If we're in a non-shooting region while the shooting function is active,
+            // set the hood to a low setting to get under the trench and prevent shooting.
+            turretSubsystem.setHoodToLaunchAngle(Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG);
+            okToShoot = false;
+        }
+        
+        // STEP 10: Run the kicker if all the calculations went ok, the turret is pointed at the target, and the motors are ready
+        // Note we don't call turretSubsystem.isLauncherUpToSpeed() because it currently does nothing.
+        if (waitCommand.isFinished() && (okToShoot)) {
+            hopperSubsystem.feedKicker();
+        } else{
+            hopperSubsystem.stopKicker();
+        }
+        Logger.recordOutput("UltraShooter-OkToShoot", okToShoot);
+
+        // OPTIONAL: Activate rumble if it's not ok to shoot
+        if (!okToShoot) {
+            // ACTIVATE CONTROLLER RUMBLE
+        } else {
+            // DEACTIVATE CONTROLLER RUMBLE
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        waitCommand.cancel();
+        hopperSubsystem.spindexerOff();
+        hopperSubsystem.stopKicker();
+
+        turretSubsystem.setLauncher(0);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+
+        Logger.recordOutput("UltraShooter-State", "INACTIVE");
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
+            } else {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
+            }
+        });
+
+        // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
+        // TODO: Deactivate rumble if we decided to use the rumble feature when the robot prevents shooting
+    }
+
+}

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

@@ -38,6 +38,7 @@ import frc.robot.Constants;
 import frc.robot.Robot;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -130,6 +131,14 @@ public class TurretSubsystem extends TestableSubsystem {
     turretState = TurretState.INIT;
   }
 
+  /**
+   * Sets the launcher motor controller (shooter flywheel motor) to achieve 
+   * the provided motor speed (in RPM) using the Spark controller's velocity control mode.
+   * The resulting shooter flywheel speed is twice the motor speed.
+   * @param speed the commanded motor speed in RPM
+   * @param distanceInMeters distance between turret and target, in meters
+   * @return nothing
+   */
   public void setLauncher(double speed) {
     if(speed == 0)
     {
@@ -146,6 +155,12 @@ public class TurretSubsystem extends TestableSubsystem {
     // return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
   }
 
+  /**
+   * Sets the hood angle when the target elevation servo position is known,
+   * from zero (lowest hood angle) to 300 (highest hood angle).
+   * @param elev the target servo motor position, from 0 to 300
+   * @return nothing
+   */
   public void setTargetElevation(double elev)
   {
     double pos = Math.abs((elev - 300)/300);
@@ -153,6 +168,31 @@ public class TurretSubsystem extends TestableSubsystem {
     hood.set(pos);
   }
 
+  /**
+   * Sets the launch (hood) angle when the target angle is known, in degrees.
+   * @param angleDegrees the commanded motor speed in RPM
+   * @return nothing
+   */
+  public void setHoodToLaunchAngle(int angleDegrees)
+  {
+    // Confirm target is within expected range. If not, do nothing.
+    if ((angleDegrees < Constants.ShooterConstants.HOOD_ANGLE_MIN_DEG) ||
+        (angleDegrees > Constants.ShooterConstants.HOOD_ANGLE_MAX_DEG)) {
+      return;
+    }
+
+    // Grab min and max values to simplify the expressions and ensure precision
+    double minAngle = (double) Constants.ShooterConstants.HOOD_ANGLE_MIN_DEG;
+    double maxAngle = (double) Constants.ShooterConstants.HOOD_ANGLE_MAX_DEG;
+    
+    // Map so that min angle maps to "1" and max angle maps to "0."
+    double pos = (maxAngle - angleDegrees) / (maxAngle - minAngle);
+
+    Logger.recordOutput("ShooterHood-Angle", pos);
+    hood.set(pos);
+  }
+
+
   public Command revSpeedCommand()
   {
     return this.runOnce(() -> setLauncher(-.5));
@@ -246,6 +286,13 @@ public class TurretSubsystem extends TestableSubsystem {
 
   }
 
+  /**
+   * Commands turret to point in a specific field direction given
+   * the current heading of the robot. 
+   * @param robotPose the robot's field-relative pose
+   * @param pos the field-relative rotations of angle to the target
+   * @return nothing
+   */
   public void setTurretFieldRelative(Pose3d robotPose, double pos)
   {
     if(turretState != TurretState.READY)
@@ -305,6 +352,24 @@ public class TurretSubsystem extends TestableSubsystem {
     return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
   }
 
+  /**
+   * Checks whether the turret can physically achieve the angle to the
+   * target based on the robot's heading.
+   * Uses Rotation2d to simplify the math.
+   * Assumes turret rotational midpoint is 180° opposite robot's heading.
+   * @param botPose the robot's field-relative pose
+   * @param targetTranslation the translation from the turret to the intended target
+   * @return true if angle is achieveable
+   */
+  public boolean isTurretAnglePossible(Pose3d botPose, Translation2d targetTranslation){
+    Rotation2d robotHeading = botPose.getRotation().toRotation2d();                           // Robot's field-relative heading (forward)
+    Rotation2d turretCenterPos = robotHeading.plus(Rotation2d.fromRotations(0.5));  // Turret's neutral azimuth
+    Rotation2d targetDirection = targetTranslation.getAngle();                                // Field-relative aiming angle
+    Rotation2d turretOffset = targetDirection.minus(turretCenterPos);                         // Difference between the two
+
+    return (Math.abs(turretOffset.getRotations()) <= (Constants.ShooterConstants.TURRET_RANGE_ROTATIONS / 2));
+  }
+
   private double getRequestedPosTurretRelative() {
     return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
   }