diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bdc74e7..532073d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 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; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fd86a44..a27e894 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/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