diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ef4d67..1ff153f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 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 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 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 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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdf7e9d..13f0865 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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.x().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().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()); } diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java index 4a0e8c8..d249a61 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/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 alliance: the red/blue alliance of the robot, as a sanity check + * @param robotPose the robot's pose + * @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 alliance: the red/blue alliance of the robot, as a sanity check + * @param region the identifier of the robot's current field region + * @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. */ diff --git a/src/main/java/frc/robot/commands/UltraShooterCommand.java b/src/main/java/frc/robot/commands/UltraShooterCommand.java new file mode 100644 index 0000000..2cc2778 --- /dev/null +++ b/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 + } + +} diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6d6a7f4..78b2313 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/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; }