From 13d728d084e9cd354362b37bd1b7c30de2a68a4b Mon Sep 17 00:00:00 2001 From: chris park Date: Wed, 28 Jan 2026 19:01:32 -0500 Subject: [PATCH 1/6] Added shooter constant for aiming --- src/main/java/frc/robot/Constants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3cfd7aa..50de69d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -114,6 +114,7 @@ public final class ShooterConstants { public static final Distance WHEEL_RADIUS = Meters.of(0.5); 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); + public static final double HEIGHT_DIFFERENCE = 1.0; } public static final int CANDLE_ID = 50; From f3099a20211ebba02300be318581cdb438dc34c8 Mon Sep 17 00:00:00 2001 From: shlap Date: Thu, 29 Jan 2026 18:37:13 -0500 Subject: [PATCH 2/6] Created rough outline for calculating shooter angle --- src/main/java/frc/robot/Constants.java | 3 ++- .../java/frc/robot/subsystems/shooter/Shooter.java | 14 ++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 50de69d..f221165 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -114,7 +114,8 @@ public final class ShooterConstants { public static final Distance WHEEL_RADIUS = Meters.of(0.5); 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); - public static final double HEIGHT_DIFFERENCE = 1.0; + public static final double HEIGHT_DIFFERENCE = 21.0; // Inches 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 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 ba3067c..cfbbbcf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; +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; @@ -16,6 +17,7 @@ public class Shooter extends SubsystemBase { private FlywheelMechanism _flywheel; private FlywheelMechanism _feeder; public double desiredVelo; + public Angle HoodAngle; public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder) { _flywheel = flywheel; @@ -42,6 +44,18 @@ public boolean flyAtVelocity() { <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; } + public double getHoodAngleDegrees(double distanceToTarget) { + + final double g = 9.81; + + double check = Math.pow(ShooterConstants.EXIT_VELOCITY, 4) - g * (g * Math.pow(distanceToTarget, 2) + 2 * ShooterConstants.HEIGHT_DIFFERENCE * Math.pow(ShooterConstants.EXIT_VELOCITY, 2)); + + if (check < 0) { + return 45.0; // Default angle if the shot is not possible + } + return Math.toDegrees(Math.atan((ShooterConstants.EXIT_VELOCITY*ShooterConstants.EXIT_VELOCITY + Math.sqrt(check)) / (g * distanceToTarget))); + } + public Command shoot(double velocity) { return Commands.run( () -> { From 8377215dbdfde216ce7dfb340e2bb1bbaef10512 Mon Sep 17 00:00:00 2001 From: shlap Date: Thu, 29 Jan 2026 19:18:23 -0500 Subject: [PATCH 3/6] Comments + Clean up + hood + logic --- src/main/java/frc/robot/Constants.java | 7 +- .../frc/robot/subsystems/shooter/Shooter.java | 70 +++++++++++++------ 2 files changed, 53 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f221165..6edc4e3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,6 +66,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); @@ -114,8 +115,12 @@ public final class ShooterConstants { public static final Distance WHEEL_RADIUS = Meters.of(0.5); 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); - public static final double HEIGHT_DIFFERENCE = 21.0; // Inches between flywheel center and top of hub opening + // 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 double HOOD_TOLERANCE = 1.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 cfbbbcf..bed4963 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,27 +1,38 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Degree; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RotationsPerSecond; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; 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 edu.wpi.first.wpilibj2.command.WaitUntilCommand; 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; - public Angle HoodAngle; + 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 @@ -32,8 +43,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); } @@ -44,33 +56,45 @@ public boolean flyAtVelocity() { <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; } - public double getHoodAngleDegrees(double distanceToTarget) { + public double getHoodAngleDegrees(Translation2d robotPos) { final double g = 9.81; - double check = Math.pow(ShooterConstants.EXIT_VELOCITY, 4) - g * (g * Math.pow(distanceToTarget, 2) + 2 * ShooterConstants.HEIGHT_DIFFERENCE * Math.pow(ShooterConstants.EXIT_VELOCITY, 2)); + //TODO: Replace with HUB later once it gets added. + double distance = robotPos.getDistance(FieldConstants.FIELDCENTER); + + double check = Math.pow(ShooterConstants.EXIT_VELOCITY, 4) - g * (g * Math.pow(distance, 2) + 2 * ShooterConstants.HEIGHT_DIFFERENCE * Math.pow(ShooterConstants.EXIT_VELOCITY, 2)); if (check < 0) { return 45.0; // Default angle if the shot is not possible } - return Math.toDegrees(Math.atan((ShooterConstants.EXIT_VELOCITY*ShooterConstants.EXIT_VELOCITY + Math.sqrt(check)) / (g * distanceToTarget))); + return Math.toDegrees(Math.atan((ShooterConstants.EXIT_VELOCITY*ShooterConstants.EXIT_VELOCITY + Math.sqrt(check)) / (g * distance))); + } + + // Sets hood angle + public void setHoodAngle(double angleDegrees) { + hoodAngle = angleDegrees; + _hood.runPosition(Angle.ofBaseUnits(angleDegrees, Degrees), ShooterConstants.HOOD_VELOCITY, ShooterConstants.HOOD_ACCELERATION, null, 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( + // start setting targets in parallel + Commands.runOnce(() -> setFlywheelVelocity(velocity)), + Commands.runOnce(() -> setHoodAngle(hoodAngle)), + // wait until both are ready + Commands.waitUntil(this::flyAtVelocity).alongWith(Commands.waitUntil(this::hoodAtAngle)), + // feed once ready + Commands.runOnce(() -> runFeeder()), + // stop flywheel when finished + Commands.runOnce(() -> setFlywheelVelocity(0))); } @Override From a5e137d5d290e56b97e3bf18af2c4517a17cf8cf Mon Sep 17 00:00:00 2001 From: shlap Date: Fri, 30 Jan 2026 22:35:44 -0500 Subject: [PATCH 4/6] Added necessary changes + spotlessApply --- src/main/java/frc/robot/Constants.java | 31 ++++++++++----- .../frc/robot/subsystems/hopper/Hopper.java | 25 ++++++------ .../frc/robot/subsystems/shooter/Shooter.java | 39 +++++++++++-------- 3 files changed, 58 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6edc4e3..643c2cf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,7 +66,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 + // TODO: Update to 2026 Field Constants and add HUB Center public static final AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); @@ -88,7 +88,8 @@ public class HopperConstants { private static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); public static final AngularVelocity ANGULAR_VELOCITY = RotationsPerSecond.of(1); - public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); + public static final AngularAcceleration ANGULAR_ACCELERATION = + RotationsPerSecondPerSecond.of(1); } public class Ports { @@ -100,7 +101,7 @@ public class Ports { } public final class ShooterConstants { - // Constants for the Shooter + // Constants for the Shooter public static final Angle ANGLE_TOLERANCE = Rotations.of(0.01); public static final AngularVelocity ANGLE_VELOCITY_TOLERANCE = RotationsPerSecond.of(0.01); public static final AngularVelocity CRUISE_VELOCITY = RotationsPerSecond.of(204); @@ -114,15 +115,19 @@ public final class ShooterConstants { public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); 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 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 double HOOD_TOLERANCE = 1.0; // degrees + 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 int CANDLE_ID = 50; public class IntakeConstants { @@ -144,7 +149,7 @@ public class FeederConstants { public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0); public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0); } - + public class ClimberConstants { public static final Distance TOLERANCE = Inches.of(0.1); public static final double GEARING = (5.0 / 1.0); @@ -153,6 +158,12 @@ public class ClimberConstants { public static final Distance STARTING_DISTANCE = Inches.of(0.0); public static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); - public static final LinearMechCharacteristics CHARACTERISTICS = new LinearMechCharacteristics(new Translation3d(0.0, 0.0, 0.0), MIN_DISTANCE, MAX_DISTANCE, STARTING_DISTANCE, CONVERTER); + public static final LinearMechCharacteristics CHARACTERISTICS = + new LinearMechCharacteristics( + new Translation3d(0.0, 0.0, 0.0), + MIN_DISTANCE, + MAX_DISTANCE, + STARTING_DISTANCE, + CONVERTER); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 95f1b8e..08d7799 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -6,23 +6,26 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; -import frc.robot.Constants; import frc.robot.Constants.HopperConstants; public class Hopper extends SubsystemBase { -private FlywheelMechanism _io; + private FlywheelMechanism _io; -public Hopper(FlywheelMechanism io) { -_io = io; -} + public Hopper(FlywheelMechanism io) { + _io = io; + } -public void setGoal(double position) { - Distance positionInches = Inches.of(position); - _io.runPosition(HopperConstants.CONVERTER.toAngle(positionInches), HopperConstants.ANGULAR_VELOCITY, HopperConstants.ANGULAR_ACCELERATION, null, PIDSlot.SLOT_0); -} + public void setGoal(double position) { + Distance positionInches = Inches.of(position); + _io.runPosition( + HopperConstants.CONVERTER.toAngle(positionInches), + HopperConstants.ANGULAR_VELOCITY, + HopperConstants.ANGULAR_ACCELERATION, + null, + PIDSlot.SLOT_0); + } @Override public void periodic() {} - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index bed4963..1cea9ad 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,17 +1,14 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RotationsPerSecond; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; 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 edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; @@ -58,23 +55,35 @@ public boolean flyAtVelocity() { public double getHoodAngleDegrees(Translation2d robotPos) { - final double g = 9.81; - - //TODO: Replace with HUB later once it gets added. + // TODO: Replace with HUB later once it gets added. double distance = robotPos.getDistance(FieldConstants.FIELDCENTER); - double check = Math.pow(ShooterConstants.EXIT_VELOCITY, 4) - g * (g * Math.pow(distance, 2) + 2 * ShooterConstants.HEIGHT_DIFFERENCE * Math.pow(ShooterConstants.EXIT_VELOCITY, 2)); + 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 45.0; // Default angle if the shot is not possible } - return Math.toDegrees(Math.atan((ShooterConstants.EXIT_VELOCITY*ShooterConstants.EXIT_VELOCITY + Math.sqrt(check)) / (g * distance))); + 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, null, PIDSlot.SLOT_0); + _hood.runPosition( + Angle.ofBaseUnits(angleDegrees, Degrees), + ShooterConstants.HOOD_VELOCITY, + ShooterConstants.HOOD_ACCELERATION, + ShooterConstants.HOOD_JERK, + PIDSlot.SLOT_0); } // Checks if hood is at angle @@ -82,21 +91,19 @@ public boolean hoodAtAngle() { return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; } - public Command shoot(double velocity) { // Prepare targets return Commands.sequence( - // start setting targets in parallel - Commands.runOnce(() -> setFlywheelVelocity(velocity)), - Commands.runOnce(() -> setHoodAngle(hoodAngle)), - // wait until both are ready - Commands.waitUntil(this::flyAtVelocity).alongWith(Commands.waitUntil(this::hoodAtAngle)), + // 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 public void periodic() {} } From 6feb2961882e7ee19140c6726c6ab03a2649206f Mon Sep 17 00:00:00 2001 From: chris park Date: Sat, 31 Jan 2026 15:25:18 -0500 Subject: [PATCH 5/6] spotless apply --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9ce768b..1599429 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,8 +4,8 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.measure.Angle; 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; From 88537ee6f5049dc8ba5169e716775d311c9ef8e6 Mon Sep 17 00:00:00 2001 From: chris park Date: Sat, 31 Jan 2026 15:34:48 -0500 Subject: [PATCH 6/6] Default hood changes --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bd7a995..532073d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -109,7 +109,6 @@ public class Ports { } public final class ShooterConstants { - // Constants for the Shooter // Constants for the Shooter public static final Angle ANGLE_TOLERANCE = Rotations.of(0.01); public static final AngularVelocity ANGLE_VELOCITY_TOLERANCE = RotationsPerSecond.of(0.01); @@ -131,6 +130,7 @@ public final class ShooterConstants { 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 @@ -140,6 +140,7 @@ public final class ShooterConstants { 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 1599429..a27e894 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -83,7 +83,7 @@ public double getHoodAngleDegrees(Translation2d robotPos) { * Math.pow(ShooterConstants.EXIT_VELOCITY, 2)); if (check < 0) { - return 45.0; // Default angle if the shot is not possible + return ShooterConstants.IDLE_HOOD_ANGLE; // Default angle if the shot is not possible } return Math.toDegrees( Math.atan(