From 876f73ad6244e57a6313f835c1a92a3f6ea44c7c Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 21 Feb 2026 12:58:52 -0500 Subject: [PATCH 1/9] Revision of launch calculation --- .../frc/robot/statemachines/DriveState.java | 9 ++ .../robot/statemachines/LaunchCalculator.java | 146 ++++++++++++++++++ .../frc/robot/statemachines/LaunchState.java | 17 ++ .../subsystems/drive/DriveConstants.java | 1 - .../shooter/LaunchRequestBuilder.java | 32 ---- .../shooter/MappedLauchRequestBuilder.java | 137 ++-------------- .../ParabolicLaunchRequestBuilder.java | 47 ++---- .../subsystems/shooter/ShooterConstants.java | 8 +- .../subsystems/shooter/ShooterSubsystem.java | 25 --- 9 files changed, 198 insertions(+), 224 deletions(-) create mode 100644 src/main/java/frc/robot/statemachines/LaunchCalculator.java create mode 100644 src/main/java/frc/robot/statemachines/LaunchState.java delete mode 100644 src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java diff --git a/src/main/java/frc/robot/statemachines/DriveState.java b/src/main/java/frc/robot/statemachines/DriveState.java index 167624e..1348be5 100644 --- a/src/main/java/frc/robot/statemachines/DriveState.java +++ b/src/main/java/frc/robot/statemachines/DriveState.java @@ -1,6 +1,8 @@ package frc.robot.statemachines; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -16,6 +18,8 @@ public class DriveState { private HashMap> concurrentQueueMap; + + private SwerveDriveState previousDriveStats = null; private SwerveDriveState currentDriveStats = null; private DriveState() { @@ -57,6 +61,7 @@ public ArrayList grabVisionEstimateList(String cameraName) { } public void adjustCurrentDriveStats(SwerveDriveState newStats) { + previousDriveStats = currentDriveStats; currentDriveStats = newStats; } @@ -68,6 +73,10 @@ public SwerveDriveState getCurrentDriveStats() { return currentDriveStats; } + public SwerveDriveState getPreviousDriveStats(){ + return previousDriveStats; + } + public ChassisSpeeds getFieldVelocity() { return ChassisSpeeds.fromRobotRelativeSpeeds( getCurrentDriveStats().Speeds, getCurrentDriveStats().Pose.getRotation()); diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java new file mode 100644 index 0000000..72742ad --- /dev/null +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -0,0 +1,146 @@ +package frc.robot.statemachines; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Pose2d; +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.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import frc.robot.statemachines.LaunchState.LaunchType; +import frc.robot.subsystems.shooter.MappedLauchRequestBuilder; +import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; + +public class LaunchCalculator { + public record LaunchRequest( + Angle launchHoodTarget, + AngularVelocity launchVelocity, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle) {} + + private Pose3d target; + private LaunchType type; + + protected LaunchCalculator(Pose3d target, LaunchType type){ + this.target = target; + this.type = type; + } + + private double loopPeriodSecs = 0.02; + + private final LinearFilter driveAngleFilter = + LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs)); + + private static final double phaseDelay; + + private static final InterpolatingDoubleTreeMap hubTimeOfFlightMap = + new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap passingTimeOfFlightMap = + new InterpolatingDoubleTreeMap(); + + // TODO: All of this is made up. Need real numbers. + static { + phaseDelay = 0.03; + + hubTimeOfFlightMap.put(5.68, 1.16); + hubTimeOfFlightMap.put(4.55, 1.12); + hubTimeOfFlightMap.put(3.15, 1.11); + hubTimeOfFlightMap.put(1.88, 1.09); + hubTimeOfFlightMap.put(1.38, 0.90); + + passingTimeOfFlightMap.put(6.68, 1.16); + passingTimeOfFlightMap.put(5.55, 1.12); + passingTimeOfFlightMap.put(4.15, 1.11); + passingTimeOfFlightMap.put(2.88, 1.09); + passingTimeOfFlightMap.put(2.38, 0.90); + } + + public LaunchRequest createLaunchRequest() { + + boolean passing = target.getZ() < 0.1; + + // current pose and movement + Pose2d estimatedRobotPose = DriveState.getInstance().getCurrentDriveStats().Pose; + ChassisSpeeds robotSpeeds = DriveState.getInstance().getCurrentDriveStats().Speeds; + + // predicted pose + estimatedRobotPose.exp( + new Twist2d( + robotSpeeds.vxMetersPerSecond * phaseDelay, + robotSpeeds.vyMetersPerSecond * phaseDelay, + robotSpeeds.omegaRadiansPerSecond * phaseDelay)); + + // TODO: for now assume they're the same. calculate offsets later + Pose2d launcherPose = estimatedRobotPose; + double launcherToTargetDistance = + launcherPose.getTranslation().getDistance(target.toPose2d().getTranslation()); + + // Calculate field relative velocity + double xVelocity = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; + double yVelocity = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; + + // Account for imparted velocity by robot (launcher) to offset + double timeOfFlight = + passing + ? passingTimeOfFlightMap.get(launcherToTargetDistance) + : hubTimeOfFlightMap.get(launcherToTargetDistance); + Pose2d lookaheadPose = launcherPose; + double lookaheadLauncherToTargetDistance = launcherToTargetDistance; + + for (int i = 0; i < 20; i++) { + timeOfFlight = + passing + ? passingTimeOfFlightMap.get(lookaheadLauncherToTargetDistance) + : hubTimeOfFlightMap.get(lookaheadLauncherToTargetDistance); + double offsetX = xVelocity * timeOfFlight; + double offsetY = yVelocity * timeOfFlight; + lookaheadPose = + new Pose2d( + launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), + launcherPose.getRotation()); + lookaheadLauncherToTargetDistance = + target + .getTranslation() + .toTranslation2d() + .getDistance(lookaheadPose.getTranslation()); + } + + // calcuate rotation angle + Rotation2d targetRobotAngle = + getDriveAngle(lookaheadPose, target.getTranslation().toTranslation2d()); + + AngularVelocity targetRobotAngularVelocity = RadiansPerSecond.of(driveAngleFilter.calculate(targetRobotAngle.minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()).getRadians() / loopPeriodSecs)); + + if(type == LaunchType.MAPPED) + return MappedLauchRequestBuilder.createLaunchRequest(passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); + else return ParabolicLaunchRequestBuilder.createLaunchRequest(passing, target.getZ(), lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); + } + + private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { + Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); + Rotation2d hubAngle = + new Rotation2d( + Math.asin( + MathUtil.clamp( + robotPose.getTranslation().getY() + / target.getDistance(robotPose.getTranslation()), + -1.0, + 1.0))); + Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation()); + return driveAngle; + } + + + +} diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java new file mode 100644 index 0000000..76b0209 --- /dev/null +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -0,0 +1,17 @@ +package frc.robot.statemachines; + +public class LaunchState { + private static LaunchState single_instance = null; + + private LaunchState(){} + + public static synchronized LaunchState getInstance() { + if (single_instance == null) single_instance = new LaunchState(); + return single_instance; + } + + public enum LaunchType{ + PARABOLIC, + MAPPED + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 31034be..0cd7fe9 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -31,5 +31,4 @@ public class DriveConstants { public static final double TRANSLATION_ALIGN_KP = 0.0001; public static final double ROTATION_ALIGN_KP = 6.28; - } diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java deleted file mode 100644 index da3f1c0..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.shooter; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import java.util.function.Supplier; - -/** Add your docs here. */ -public abstract class LaunchRequestBuilder { - - public record LaunchRequest( - Angle launchHoodTarget, - AngularVelocity launchVelocity, - LinearVelocity driveVelocity, - Rotation2d driveAngle) {} - - Supplier targetPose; - - protected LaunchRequestBuilder(Supplier targetPose) { - this.targetPose = targetPose; - } - - public abstract LaunchRequestBuilder getInstance(Supplier targetPose); - - public abstract LaunchRequest createLaunchRequest(); -} diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java index aa60239..0fcf5b2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java @@ -24,60 +24,28 @@ import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.statemachines.DriveState; import java.util.function.Supplier; +import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ -public class MappedLauchRequestBuilder extends LaunchRequestBuilder { +public class MappedLauchRequestBuilder{ - private MappedLauchRequestBuilder instance; - - private MappedLauchRequestBuilder(Supplier targetPose) { - super(targetPose); - } - - private LaunchRequest currentLaunchRequest; - - @Override - public MappedLauchRequestBuilder getInstance(Supplier targetPose) { - if (this.instance != null) { - instance = new MappedLauchRequestBuilder(targetPose); - return instance; - } else { - this.targetPose = targetPose; - return instance; - } - } - - private double loopPeriodSecs = 0.02; - - private double lastHoodAngle; - private Rotation2d lastDriveAngle; - - private final LinearFilter hoodAngleFilter = - LinearFilter.movingAverage((int) (0.1 / loopPeriodSecs)); - private final LinearFilter driveAngleFilter = - LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs)); private static final double minDistance; private static final double maxDistance; private static final double passingMinDistance; private static final double passingMaxDistance; - private static final double phaseDelay; // Launching Maps private static final InterpolatingTreeMap hoodAngleMap = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); private static final InterpolatingDoubleTreeMap flywheelSpeedMap = new InterpolatingDoubleTreeMap(); - private static final InterpolatingDoubleTreeMap timeOfFlightMap = - new InterpolatingDoubleTreeMap(); // Passing Maps private static final InterpolatingTreeMap passingHoodAngleMap = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); private static final InterpolatingDoubleTreeMap passingFlywheelSpeedMap = new InterpolatingDoubleTreeMap(); - private static final InterpolatingDoubleTreeMap passingTimeOfFlightMap = - new InterpolatingDoubleTreeMap(); // TODO: All of this is made up. Need real numbers. static { @@ -85,7 +53,6 @@ public MappedLauchRequestBuilder getInstance(Supplier targetPose) { maxDistance = 5.60; passingMinDistance = 0.0; passingMaxDistance = 100000; - phaseDelay = 0.03; hoodAngleMap.put(1.34, Rotation2d.fromDegrees(19.0)); hoodAngleMap.put(1.78, Rotation2d.fromDegrees(19.0)); @@ -109,116 +76,32 @@ public MappedLauchRequestBuilder getInstance(Supplier targetPose) { flywheelSpeedMap.put(5.57, 275.0); flywheelSpeedMap.put(5.60, 290.0); - timeOfFlightMap.put(5.68, 1.16); - timeOfFlightMap.put(4.55, 1.12); - timeOfFlightMap.put(3.15, 1.11); - timeOfFlightMap.put(1.88, 1.09); - timeOfFlightMap.put(1.38, 0.90); passingHoodAngleMap.put(passingMinDistance, Rotation2d.fromDegrees(0.0)); passingHoodAngleMap.put(passingMaxDistance, Rotation2d.fromDegrees(0.0)); passingFlywheelSpeedMap.put(passingMinDistance, 0.0); passingFlywheelSpeedMap.put(passingMaxDistance, 0.0); - - passingTimeOfFlightMap.put(passingMinDistance, 0.0); - passingTimeOfFlightMap.put(passingMaxDistance, 0.0); } - @Override - public LaunchRequest createLaunchRequest() { - - boolean passing = targetPose.get().getZ() == 0.0; // target pose is the floor, so we're passing. - - // current pose and movement - Pose2d estimatedRobotPose = DriveState.getInstance().getCurrentDriveStats().Pose; - ChassisSpeeds robotSpeeds = DriveState.getInstance().getCurrentDriveStats().Speeds; - - // predicted pose - estimatedRobotPose.exp( - new Twist2d( - robotSpeeds.vxMetersPerSecond * phaseDelay, - robotSpeeds.vyMetersPerSecond * phaseDelay, - robotSpeeds.omegaRadiansPerSecond * phaseDelay)); - - // TODO: for now assume they're the same. calculate offsets later - Pose2d launcherPose = estimatedRobotPose; - double launcherToTargetDistance = - launcherPose.getTranslation().getDistance(targetPose.get().toPose2d().getTranslation()); - - // Calculate field relative velocity - double xVelocity = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; - double yVelocity = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; - - // Account for imparted velocity by robot (launcher) to offset - double timeOfFlight = - passing - ? passingTimeOfFlightMap.get(launcherToTargetDistance) - : timeOfFlightMap.get(launcherToTargetDistance); - Pose2d lookaheadPose = launcherPose; - double lookaheadLauncherToTargetDistance = launcherToTargetDistance; - - for (int i = 0; i < 20; i++) { - timeOfFlight = - passing - ? passingTimeOfFlightMap.get(lookaheadLauncherToTargetDistance) - : timeOfFlightMap.get(lookaheadLauncherToTargetDistance); - double offsetX = xVelocity * timeOfFlight; - double offsetY = yVelocity * timeOfFlight; - lookaheadPose = - new Pose2d( - launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), - launcherPose.getRotation()); - lookaheadLauncherToTargetDistance = - targetPose - .get() - .getTranslation() - .toTranslation2d() - .getDistance(lookaheadPose.getTranslation()); - } - - // calcuate rotation angle - Rotation2d driveAngle = - getDriveAngle(lookaheadPose, targetPose.get().getTranslation().toTranslation2d()); + public static LaunchRequest createLaunchRequest(boolean passing, double distance, AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle) { // calculate hood angle double hoodAngle = passing - ? passingHoodAngleMap.get(lookaheadLauncherToTargetDistance).getRadians() - : hoodAngleMap.get(lookaheadLauncherToTargetDistance).getRadians(); + ? passingHoodAngleMap.get(distance).getRadians() + : hoodAngleMap.get(distance).getRadians(); // calculate flywheel speed double flywheelSpeed = passing - ? passingFlywheelSpeedMap.get(lookaheadLauncherToTargetDistance) - : flywheelSpeedMap.get(lookaheadLauncherToTargetDistance); - - // filter for smoothing - double driveVelocity = - driveAngleFilter.calculate(driveAngle.minus(lastDriveAngle).getRadians() / loopPeriodSecs); - lastDriveAngle = driveAngle; + ? passingFlywheelSpeedMap.get(distance) + : flywheelSpeedMap.get(distance); - currentLaunchRequest = - new LaunchRequest( + return new LaunchRequest( Angle.ofBaseUnits(hoodAngle, Radians), AngularVelocity.ofBaseUnits(flywheelSpeed, RadiansPerSecond), - LinearVelocity.ofBaseUnits(driveVelocity, MetersPerSecond), - driveAngle); - - return currentLaunchRequest; - } - - private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { - Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); - Rotation2d hubAngle = - new Rotation2d( - Math.asin( - MathUtil.clamp( - robotPose.getTranslation().getY() - / target.getDistance(robotPose.getTranslation()), - -1.0, - 1.0))); - Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation()); - return driveAngle; + targetRobotAngularVelocity, + targetRobotAngle); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index a15cc80..25c1ee1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -7,45 +7,26 @@ import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.statemachines.DriveState; import java.util.function.Supplier; +import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ -public class ParabolicLaunchRequestBuilder extends LaunchRequestBuilder { +public class ParabolicLaunchRequestBuilder { - private ParabolicLaunchRequestBuilder instance; - - private ParabolicLaunchRequestBuilder(Supplier targetPose) { - super(targetPose); - } - - @Override - public ParabolicLaunchRequestBuilder getInstance(Supplier targetPose) { - if (this.instance != null) { - instance = new ParabolicLaunchRequestBuilder(targetPose); - return instance; - } else { - this.targetPose = targetPose; - return instance; - } - } - - @Override - public LaunchRequest createLaunchRequest() { + public static LaunchRequest createLaunchRequest(boolean passing, double goalHeight, double distance, AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); - double x2 = - DriveState.getInstance() - .getCurrentDriveStats() - .Pose - .getTranslation() - .getDistance(targetPose.get().getTranslation().toTranslation2d()); - // double y2 = ShooterConstants.GOAL_HEIGHT.in(Meters); - double y2 = targetPose.get().getMeasureZ().in(Meters); + double x2 = distance; + double y2 = goalHeight; - double slope = ShooterConstants.OPTIMAL_ENTRY_SLOPE; + double slope; + if(passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; + else slope = ShooterConstants.OPTIMAL_HUB_ENTRY_SLOPE; + double a, b, vertex; Angle theta, motorAngle; do { @@ -58,7 +39,7 @@ public LaunchRequest createLaunchRequest() { motorAngle = Radians.of(Math.PI / 2 - theta.in(Radians)); vertex = -1 * b / (2 * a); slope -= 0.05; - } while ((vertex > x2 - ShooterConstants.MIN_VERTEX_DISTANCE.in(Meters)) + } while ((passing || vertex > x2 - ShooterConstants.MIN_HUB_VERTEX_DISTANCE.in(Meters)) && !(motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees))); if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null; @@ -78,10 +59,6 @@ public LaunchRequest createLaunchRequest() { // Create a typed AngularVelocity object (optional, for use within the units library ecosystem) AngularVelocity angularVelocity = RadiansPerSecond.of(angularVelocityMagnitude); - return new LaunchRequest( - theta, - angularVelocity, - LinearVelocity.ofBaseUnits(0, MetersPerSecond), - DriveState.getInstance().getCurrentDriveStats().Pose.getRotation()); + return new LaunchRequest(theta, angularVelocity, targetRobotAngularVelocity, targetRobotAngle); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 0273c99..10c6a5a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -68,9 +68,9 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() // Lemon Yeeting Constants public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate - public static final Distance GOAL_HEIGHT = Inch.of(23); // TODO: Get Better Estimate - public static final Distance OFFSET_DISTANCE = Meter.of(1); // TODO: Get Better Estimate - public static final Distance MIN_VERTEX_DISTANCE = Inch.of(23.5); + + public static final Distance MIN_HUB_VERTEX_DISTANCE = Inch.of(23.5); public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate - public static final double OPTIMAL_ENTRY_SLOPE = -1; // TODO: Tune + public static final double OPTIMAL_PASSING_ENTRY_SLOPE = -1; // TODO: Tune + public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; //TODO: Tune } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index bb6279e..99a9bc3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -42,31 +42,6 @@ public class ShooterSubsystem extends SubsystemBase { state -> SignalLogger.writeString("SysIdFlywheel_State", state.toString())), new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); - public class LaunchRequest { - private Angle launchHoodTarget; - private AngularVelocity launchVelocity; - - public LaunchRequest(Angle theta, LinearVelocity velocity) { - // hood angle in degrees (0 degrees is all the way back) is converted to motor rotations - launchHoodTarget = - Rotations.of( - theta.in(Degrees) * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in(Rotations)); - // meters per second is converted to rotations per second of the flywheel - launchVelocity = - RotationsPerSecond.of( - velocity.in(MetersPerSecond) - / (2 * Math.PI * ShooterConstants.FLYWHEEL_RADIUS.in(Meters))); - } - - public Angle getHoodTarget() { - return launchHoodTarget; - } - - public AngularVelocity getVelocityTarget() { - return launchVelocity; - } - } - public ShooterSubsystem() { flywheelMotor = new TalonFX(ShooterConstants.FLYWHEEL_MOTOR_ID); hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); From dee92ac308444399ef761eb85718400c41524155 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 21 Feb 2026 13:07:00 -0500 Subject: [PATCH 2/9] a little clean up (spotless) --- .../frc/robot/statemachines/DriveState.java | 5 +- .../robot/statemachines/LaunchCalculator.java | 62 ++++++++++--------- .../frc/robot/statemachines/LaunchState.java | 20 +++--- .../shooter/MappedLauchRequestBuilder.java | 33 ++++------ .../ParabolicLaunchRequestBuilder.java | 14 +++-- .../subsystems/shooter/ShooterConstants.java | 2 +- .../subsystems/shooter/ShooterSubsystem.java | 1 - 7 files changed, 63 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc/robot/statemachines/DriveState.java b/src/main/java/frc/robot/statemachines/DriveState.java index 1348be5..f586e1e 100644 --- a/src/main/java/frc/robot/statemachines/DriveState.java +++ b/src/main/java/frc/robot/statemachines/DriveState.java @@ -1,8 +1,6 @@ package frc.robot.statemachines; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -18,7 +16,6 @@ public class DriveState { private HashMap> concurrentQueueMap; - private SwerveDriveState previousDriveStats = null; private SwerveDriveState currentDriveStats = null; @@ -73,7 +70,7 @@ public SwerveDriveState getCurrentDriveStats() { return currentDriveStats; } - public SwerveDriveState getPreviousDriveStats(){ + public SwerveDriveState getPreviousDriveStats() { return previousDriveStats; } diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 72742ad..2071f1d 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -2,8 +2,6 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; -import java.util.function.Supplier; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; @@ -12,30 +10,27 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.interpolation.InterpolatingTreeMap; -import edu.wpi.first.math.interpolation.InverseInterpolator; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.statemachines.LaunchState.LaunchType; import frc.robot.subsystems.shooter.MappedLauchRequestBuilder; import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; public class LaunchCalculator { - public record LaunchRequest( - Angle launchHoodTarget, - AngularVelocity launchVelocity, - AngularVelocity targetRobotAngularVelocity, - Rotation2d targetRobotAngle) {} - - private Pose3d target; - private LaunchType type; - - protected LaunchCalculator(Pose3d target, LaunchType type){ - this.target = target; - this.type = type; - } + public record LaunchRequest( + Angle launchHoodTarget, + AngularVelocity launchVelocity, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle) {} + + private Pose3d target; + private LaunchType type; + + protected LaunchCalculator(Pose3d target, LaunchType type) { + this.target = target; + this.type = type; + } private double loopPeriodSecs = 0.02; @@ -110,21 +105,31 @@ public LaunchRequest createLaunchRequest() { launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), launcherPose.getRotation()); lookaheadLauncherToTargetDistance = - target - .getTranslation() - .toTranslation2d() - .getDistance(lookaheadPose.getTranslation()); + target.getTranslation().toTranslation2d().getDistance(lookaheadPose.getTranslation()); } // calcuate rotation angle Rotation2d targetRobotAngle = getDriveAngle(lookaheadPose, target.getTranslation().toTranslation2d()); - AngularVelocity targetRobotAngularVelocity = RadiansPerSecond.of(driveAngleFilter.calculate(targetRobotAngle.minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()).getRadians() / loopPeriodSecs)); - - if(type == LaunchType.MAPPED) - return MappedLauchRequestBuilder.createLaunchRequest(passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); - else return ParabolicLaunchRequestBuilder.createLaunchRequest(passing, target.getZ(), lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); + AngularVelocity targetRobotAngularVelocity = + RadiansPerSecond.of( + driveAngleFilter.calculate( + targetRobotAngle + .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()) + .getRadians() + / loopPeriodSecs)); + + if (type == LaunchType.MAPPED) + return MappedLauchRequestBuilder.createLaunchRequest( + passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); + else + return ParabolicLaunchRequestBuilder.createLaunchRequest( + passing, + target.getZ(), + lookaheadLauncherToTargetDistance, + targetRobotAngularVelocity, + targetRobotAngle); } private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { @@ -140,7 +145,4 @@ private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation()); return driveAngle; } - - - } diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java index 76b0209..d517e09 100644 --- a/src/main/java/frc/robot/statemachines/LaunchState.java +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -1,17 +1,17 @@ package frc.robot.statemachines; public class LaunchState { - private static LaunchState single_instance = null; + private static LaunchState single_instance = null; - private LaunchState(){} + private LaunchState() {} - public static synchronized LaunchState getInstance() { - if (single_instance == null) single_instance = new LaunchState(); - return single_instance; - } + public static synchronized LaunchState getInstance() { + if (single_instance == null) single_instance = new LaunchState(); + return single_instance; + } - public enum LaunchType{ - PARABOLIC, - MAPPED - } + public enum LaunchType { + PARABOLIC, + MAPPED + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java index 0fcf5b2..2d94759 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java @@ -4,31 +4,19 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Pose2d; -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.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.interpolation.InverseInterpolator; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.statemachines.DriveState; -import java.util.function.Supplier; import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ -public class MappedLauchRequestBuilder{ - +public class MappedLauchRequestBuilder { private static final double minDistance; private static final double maxDistance; @@ -76,7 +64,6 @@ public class MappedLauchRequestBuilder{ flywheelSpeedMap.put(5.57, 275.0); flywheelSpeedMap.put(5.60, 290.0); - passingHoodAngleMap.put(passingMinDistance, Rotation2d.fromDegrees(0.0)); passingHoodAngleMap.put(passingMaxDistance, Rotation2d.fromDegrees(0.0)); @@ -84,7 +71,11 @@ public class MappedLauchRequestBuilder{ passingFlywheelSpeedMap.put(passingMaxDistance, 0.0); } - public static LaunchRequest createLaunchRequest(boolean passing, double distance, AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle) { + public static LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle) { // calculate hood angle double hoodAngle = @@ -94,14 +85,12 @@ public static LaunchRequest createLaunchRequest(boolean passing, double distance // calculate flywheel speed double flywheelSpeed = - passing - ? passingFlywheelSpeedMap.get(distance) - : flywheelSpeedMap.get(distance); + passing ? passingFlywheelSpeedMap.get(distance) : flywheelSpeedMap.get(distance); return new LaunchRequest( - Angle.ofBaseUnits(hoodAngle, Radians), - AngularVelocity.ofBaseUnits(flywheelSpeed, RadiansPerSecond), - targetRobotAngularVelocity, - targetRobotAngle); + Angle.ofBaseUnits(hoodAngle, Radians), + AngularVelocity.ofBaseUnits(flywheelSpeed, RadiansPerSecond), + targetRobotAngularVelocity, + targetRobotAngle); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index 25c1ee1..04c0eda 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -6,27 +6,29 @@ import static edu.wpi.first.units.Units.*; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.statemachines.DriveState; -import java.util.function.Supplier; import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ public class ParabolicLaunchRequestBuilder { - public static LaunchRequest createLaunchRequest(boolean passing, double goalHeight, double distance, AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle) { + public static LaunchRequest createLaunchRequest( + boolean passing, + double goalHeight, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); double x2 = distance; double y2 = goalHeight; double slope; - if(passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; + if (passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; else slope = ShooterConstants.OPTIMAL_HUB_ENTRY_SLOPE; - + double a, b, vertex; Angle theta, motorAngle; do { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 10c6a5a..02e5abd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -72,5 +72,5 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() public static final Distance MIN_HUB_VERTEX_DISTANCE = Inch.of(23.5); public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate public static final double OPTIMAL_PASSING_ENTRY_SLOPE = -1; // TODO: Tune - public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; //TODO: Tune + public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; // TODO: Tune } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 99a9bc3..f50fee8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -10,7 +10,6 @@ import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; From 54d2e3f772bf4cb446f5dceb559f7d836d105928 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Mon, 23 Feb 2026 20:40:53 -0500 Subject: [PATCH 3/9] Implemented Interface --- src/main/java/frc/robot/statemachines/DriveState.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/statemachines/DriveState.java b/src/main/java/frc/robot/statemachines/DriveState.java index f586e1e..11ad5d8 100644 --- a/src/main/java/frc/robot/statemachines/DriveState.java +++ b/src/main/java/frc/robot/statemachines/DriveState.java @@ -16,8 +16,8 @@ public class DriveState { private HashMap> concurrentQueueMap; - private SwerveDriveState previousDriveStats = null; - private SwerveDriveState currentDriveStats = null; + private SwerveDriveState previousDriveStats = new SwerveDriveState(); + private SwerveDriveState currentDriveStats = new SwerveDriveState(); private DriveState() { concurrentQueueMap = new HashMap>(); From 7efe4e473ed62a542fc0f01e13b549187da5862b Mon Sep 17 00:00:00 2001 From: William Crouch Date: Mon, 23 Feb 2026 20:41:17 -0500 Subject: [PATCH 4/9] Actually implemented interface --- .../robot/statemachines/LaunchCalculator.java | 27 +++++++------------ .../shooter/LaunchRequestBuilder.java | 11 ++++++++ ...r.java => MappedLaunchRequestBuilder.java} | 4 +-- .../ParabolicLaunchRequestBuilder.java | 7 +++-- .../subsystems/shooter/ShooterConstants.java | 1 + 5 files changed, 26 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java rename src/main/java/frc/robot/subsystems/shooter/{MappedLauchRequestBuilder.java => MappedLaunchRequestBuilder.java} (96%) diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 2071f1d..084a90b 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -14,8 +14,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.statemachines.LaunchState.LaunchType; -import frc.robot.subsystems.shooter.MappedLauchRequestBuilder; -import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; +import frc.robot.subsystems.shooter.LaunchRequestBuilder; public class LaunchCalculator { public record LaunchRequest( @@ -25,11 +24,11 @@ public record LaunchRequest( Rotation2d targetRobotAngle) {} private Pose3d target; - private LaunchType type; + private LaunchRequestBuilder builder; - protected LaunchCalculator(Pose3d target, LaunchType type) { + protected LaunchCalculator(Pose3d target, LaunchRequestBuilder builder) { this.target = target; - this.type = type; + this.builder = builder; } private double loopPeriodSecs = 0.02; @@ -82,8 +81,8 @@ public LaunchRequest createLaunchRequest() { launcherPose.getTranslation().getDistance(target.toPose2d().getTranslation()); // Calculate field relative velocity - double xVelocity = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; - double yVelocity = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; + double xSpeed = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; + double ySpeed = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; // Account for imparted velocity by robot (launcher) to offset double timeOfFlight = @@ -98,8 +97,8 @@ public LaunchRequest createLaunchRequest() { passing ? passingTimeOfFlightMap.get(lookaheadLauncherToTargetDistance) : hubTimeOfFlightMap.get(lookaheadLauncherToTargetDistance); - double offsetX = xVelocity * timeOfFlight; - double offsetY = yVelocity * timeOfFlight; + double offsetX = xSpeed * timeOfFlight; + double offsetY = ySpeed * timeOfFlight; lookaheadPose = new Pose2d( launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), @@ -120,16 +119,8 @@ public LaunchRequest createLaunchRequest() { .getRadians() / loopPeriodSecs)); - if (type == LaunchType.MAPPED) - return MappedLauchRequestBuilder.createLaunchRequest( + return builder.createLaunchRequest( passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); - else - return ParabolicLaunchRequestBuilder.createLaunchRequest( - passing, - target.getZ(), - lookaheadLauncherToTargetDistance, - targetRobotAngularVelocity, - targetRobotAngle); } private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java new file mode 100644 index 0000000..5acd02e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.shooter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.statemachines.LaunchCalculator.LaunchRequest; + +public interface LaunchRequestBuilder { + public LaunchRequest createLaunchRequest( boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java similarity index 96% rename from src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java rename to src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index 2d94759..7d1e98f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -16,7 +16,7 @@ import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ -public class MappedLauchRequestBuilder { +public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { private static final double minDistance; private static final double maxDistance; @@ -71,7 +71,7 @@ public class MappedLauchRequestBuilder { passingFlywheelSpeedMap.put(passingMaxDistance, 0.0); } - public static LaunchRequest createLaunchRequest( + public LaunchRequest createLaunchRequest( boolean passing, double distance, AngularVelocity targetRobotAngularVelocity, diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index 04c0eda..e3fd013 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -13,17 +13,16 @@ import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ -public class ParabolicLaunchRequestBuilder { +public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder{ - public static LaunchRequest createLaunchRequest( + public LaunchRequest createLaunchRequest( boolean passing, - double goalHeight, double distance, AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); double x2 = distance; - double y2 = goalHeight; + double y2 = passing ? 0 : ShooterConstants.GOAL_HEIGHT.in(Meters); double slope; if (passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 02e5abd..d51d2a2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -68,6 +68,7 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() // Lemon Yeeting Constants public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate + public static final Distance GOAL_HEIGHT = Inch.of(23); // TODO: Get Better Estimate public static final Distance MIN_HUB_VERTEX_DISTANCE = Inch.of(23.5); public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate From d2dcf59e2841c6dc8cd1b6bd9f353e924f9b7122 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 25 Feb 2026 19:07:41 -0500 Subject: [PATCH 5/9] Activation added --- src/main/java/frc/robot/RobotContainer.java | 3 +++ .../robot/statemachines/LaunchCalculator.java | 5 ++-- .../frc/robot/statemachines/LaunchState.java | 23 +++++++++++++++++++ .../shooter/LaunchRequestBuilder.java | 4 +++- .../ParabolicLaunchRequestBuilder.java | 2 +- 5 files changed, 32 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c61fc15..ffa1afb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.commands.DriveBySpeed; import frc.robot.commands.WheelSlipTest; +import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DrivePreferences; @@ -46,6 +47,8 @@ public class RobotContainer { private final SendableChooser autoChooser; + private final LaunchState launchState = LaunchState.getInstance(); + public RobotContainer() { NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 084a90b..31f4e5c 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.statemachines.LaunchState.LaunchType; import frc.robot.subsystems.shooter.LaunchRequestBuilder; public class LaunchCalculator { @@ -119,8 +118,8 @@ public LaunchRequest createLaunchRequest() { .getRadians() / loopPeriodSecs)); - return builder.createLaunchRequest( - passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); + return builder.createLaunchRequest( + passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); } private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java index d517e09..4840cef 100644 --- a/src/main/java/frc/robot/statemachines/LaunchState.java +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -1,8 +1,15 @@ package frc.robot.statemachines; +import edu.wpi.first.math.geometry.Pose3d; +import frc.robot.statemachines.LaunchCalculator.LaunchRequest; +import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; +import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; + public class LaunchState { private static LaunchState single_instance = null; + private LaunchCalculator currentCalculator = null; + private LaunchState() {} public static synchronized LaunchState getInstance() { @@ -10,6 +17,22 @@ public static synchronized LaunchState getInstance() { return single_instance; } + public void activateCalculator(Pose3d target, LaunchType calculatorType){ + if(calculatorType == LaunchType.PARABOLIC) currentCalculator = new LaunchCalculator(target, new ParabolicLaunchRequestBuilder()); + else if(calculatorType == LaunchType.MAPPED) currentCalculator = new LaunchCalculator(target, new MappedLaunchRequestBuilder()); + } + + public void deactivateCalculator(){ + currentCalculator = null; + } + + public boolean isActivated(){return currentCalculator != null;} + + public LaunchRequest getLaunchRequest(){ + if(!isActivated()) return null; + else return currentCalculator.createLaunchRequest(); + } + public enum LaunchType { PARABOLIC, MAPPED diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java index 5acd02e..4d0927a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -1,10 +1,12 @@ package frc.robot.subsystems.shooter; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.statemachines.LaunchCalculator.LaunchRequest; public interface LaunchRequestBuilder { - public LaunchRequest createLaunchRequest( boolean passing, + public LaunchRequest createLaunchRequest( + boolean passing, double distance, AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle); diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index e3fd013..7a67ce7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -13,7 +13,7 @@ import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ -public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder{ +public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder { public LaunchRequest createLaunchRequest( boolean passing, From 9553c8e2369d5fb2d493aded02a2f7138f5fb47b Mon Sep 17 00:00:00 2001 From: William Crouch Date: Thu, 26 Feb 2026 20:29:08 -0500 Subject: [PATCH 6/9] Shooter periodic utilized (does not crash) --- simgui-ds.json | 5 ++ src/main/java/frc/robot/RobotContainer.java | 28 ++++++--- .../robot/statemachines/LaunchCalculator.java | 7 +-- .../frc/robot/statemachines/LaunchState.java | 37 ++++++++---- .../subsystems/shooter/LaunchRequest.java | 59 +++++++++++++++++++ .../shooter/LaunchRequestBuilder.java | 1 - .../shooter/MappedLaunchRequestBuilder.java | 5 +- .../ParabolicLaunchRequestBuilder.java | 9 ++- .../subsystems/shooter/ShooterSubsystem.java | 10 +++- 9 files changed, 132 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ffa1afb..67612b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,21 +8,21 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import frc.robot.commands.DriveBySpeed; -import frc.robot.commands.WheelSlipTest; import frc.robot.statemachines.LaunchState; +import frc.robot.statemachines.LaunchState.LaunchType; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.subsystems.drive.DrivePreferences; import frc.robot.subsystems.drive.DrivetrainSubsystem; import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; @Logged public class RobotContainer { @@ -42,12 +42,15 @@ public class RobotContainer { @Logged(name = "Indexer") public final IndexerSubsystem indexer = new IndexerSubsystem(); + @Logged(name = "Shooter") + public final ShooterSubsystem shooter = new ShooterSubsystem(); + @Logged(name = "Climber") public final ClimberSubsystem climber = new ClimberSubsystem(); - private final SendableChooser autoChooser; + private final LaunchState launchState = LaunchState.getInstance(); - private final LaunchState launchState = LaunchState.getInstance(); + private final SendableChooser autoChooser; public RobotContainer() { NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); @@ -84,6 +87,8 @@ private void configureSubsystemDefaultCommands() { } private void configureBindings() { + + /* // Idle while the robot is disabled. This ensures the configured // neutral mode is applied to the drive motors while disabled. final var idle = new SwerveRequest.Idle(); @@ -113,6 +118,15 @@ private void configureBindings() { joystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.registerTelemetry(logger::telemeterize); + */ + + joystick + .x() + .onTrue( + Commands.runOnce( + () -> + launchState.activateCalculator( + new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), LaunchType.PARABOLIC))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 31f4e5c..a3d6a2c 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -11,16 +11,11 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.subsystems.shooter.LaunchRequest; import frc.robot.subsystems.shooter.LaunchRequestBuilder; public class LaunchCalculator { - public record LaunchRequest( - Angle launchHoodTarget, - AngularVelocity launchVelocity, - AngularVelocity targetRobotAngularVelocity, - Rotation2d targetRobotAngle) {} private Pose3d target; private LaunchRequestBuilder builder; diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java index 4840cef..07324e4 100644 --- a/src/main/java/frc/robot/statemachines/LaunchState.java +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -1,7 +1,8 @@ package frc.robot.statemachines; +import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Pose3d; -import frc.robot.statemachines.LaunchCalculator.LaunchRequest; +import frc.robot.subsystems.shooter.LaunchRequest; import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; @@ -10,6 +11,8 @@ public class LaunchState { private LaunchCalculator currentCalculator = null; + private LaunchRequest currentRequest = null; + private LaunchState() {} public static synchronized LaunchState getInstance() { @@ -17,20 +20,34 @@ public static synchronized LaunchState getInstance() { return single_instance; } - public void activateCalculator(Pose3d target, LaunchType calculatorType){ - if(calculatorType == LaunchType.PARABOLIC) currentCalculator = new LaunchCalculator(target, new ParabolicLaunchRequestBuilder()); - else if(calculatorType == LaunchType.MAPPED) currentCalculator = new LaunchCalculator(target, new MappedLaunchRequestBuilder()); + public void activateCalculator(Pose3d target, LaunchType calculatorType) { + if (currentCalculator != null) return; + + if (calculatorType == LaunchType.PARABOLIC) + currentCalculator = new LaunchCalculator(target, new ParabolicLaunchRequestBuilder()); + else if (calculatorType == LaunchType.MAPPED) + currentCalculator = new LaunchCalculator(target, new MappedLaunchRequestBuilder()); + + refreshRequest(); } - public void deactivateCalculator(){ + public void deactivateCalculator() { currentCalculator = null; } - - public boolean isActivated(){return currentCalculator != null;} - public LaunchRequest getLaunchRequest(){ - if(!isActivated()) return null; - else return currentCalculator.createLaunchRequest(); + public boolean isActivated() { + return currentCalculator != null; + } + + public LaunchRequest getLaunchRequest() { + if (!isActivated()) return null; + + if (currentRequest.getTimestamp() + 0.02 < Utils.getCurrentTimeSeconds()) refreshRequest(); + return currentRequest; + } + + private void refreshRequest() { + currentRequest = currentCalculator.createLaunchRequest(); } public enum LaunchType { diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java new file mode 100644 index 0000000..45371e7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public class LaunchRequest { + private Angle launchHoodTarget; + private AngularVelocity launchVelocity; + private AngularVelocity targetRobotAngularVelocity; + private Rotation2d targetRobotAngle; + private double timestamp; + + public LaunchRequest( + Angle launchHoodTarget, + AngularVelocity launchVelocity, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle, + double timestamp) { + this.launchHoodTarget = + Rotations.of( + launchHoodTarget.in(Radians) + * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in( + Rotations)); // converts froms radians to rotations + this.launchVelocity = + RotationsPerSecond.of( + launchVelocity.in(RadiansPerSecond) + / Math.PI + * 2); // converts from Radians Per Second to Rotations Per Second + this.targetRobotAngularVelocity = targetRobotAngularVelocity; + this.targetRobotAngle = targetRobotAngle; + this.timestamp = timestamp; + } + + public Angle getHoodTarget() { + return launchHoodTarget; + } + + public AngularVelocity getFlywheelVelocity() { + return launchVelocity; + } + + public AngularVelocity getTargetRobotAngularVelocity() { + return targetRobotAngularVelocity; + } + + public Rotation2d getTargetRobotAngle() { + return targetRobotAngle; + } + + public double getTimestamp() { + return timestamp; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java index 4d0927a..6107652 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.statemachines.LaunchCalculator.LaunchRequest; public interface LaunchRequestBuilder { public LaunchRequest createLaunchRequest( diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index 7d1e98f..248d3bf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -7,13 +7,13 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; +import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.interpolation.InverseInterpolator; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { @@ -91,6 +91,7 @@ public LaunchRequest createLaunchRequest( Angle.ofBaseUnits(hoodAngle, Radians), AngularVelocity.ofBaseUnits(flywheelSpeed, RadiansPerSecond), targetRobotAngularVelocity, - targetRobotAngle); + targetRobotAngle, + Utils.getCurrentTimeSeconds()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index 7a67ce7..e0030d1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -6,11 +6,11 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.statemachines.LaunchCalculator.LaunchRequest; /** Add your docs here. */ public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder { @@ -60,6 +60,11 @@ public LaunchRequest createLaunchRequest( // Create a typed AngularVelocity object (optional, for use within the units library ecosystem) AngularVelocity angularVelocity = RadiansPerSecond.of(angularVelocityMagnitude); - return new LaunchRequest(theta, angularVelocity, targetRobotAngularVelocity, targetRobotAngle); + return new LaunchRequest( + motorAngle, + angularVelocity, + targetRobotAngularVelocity, + targetRobotAngle, + Utils.getCurrentTimeSeconds()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index f50fee8..d5d1138 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.intake.IntakeConstants; @Logged @@ -31,6 +32,8 @@ public class ShooterSubsystem extends SubsystemBase { private PositionTorqueCurrentFOC hoodControl; + private final LaunchState launchState = LaunchState.getInstance(); + final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( new SysIdRoutine.Config( @@ -57,8 +60,13 @@ public ShooterSubsystem() { @Override public void periodic() { + if (launchState.isActivated()) { + hoodTarget = launchState.getLaunchRequest().getHoodTarget(); + velocityTarget = launchState.getLaunchRequest().getFlywheelVelocity(); + } + flywheelMotor.setControl(velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - hoodMotor.setControl(hoodControl.withVelocity(hoodTarget.in(Rotations))); + hoodMotor.setControl(hoodControl.withPosition(hoodTarget.in(Rotations))); } public Command spinFlywheelCommand() { From f2a6a7089a170858eacd1448ccc6428d493b47ac Mon Sep 17 00:00:00 2001 From: William Crouch Date: Thu, 26 Feb 2026 21:13:01 -0500 Subject: [PATCH 7/9] Logging and parabolic debugging --- .../java/frc/robot/statemachines/LaunchState.java | 13 +++++++++++++ .../frc/robot/subsystems/shooter/LaunchRequest.java | 4 ++-- .../shooter/ParabolicLaunchRequestBuilder.java | 5 +++-- .../robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 4 files changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java index 07324e4..33d1f7f 100644 --- a/src/main/java/frc/robot/statemachines/LaunchState.java +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -1,7 +1,11 @@ package frc.robot.statemachines; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.shooter.LaunchRequest; import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; @@ -48,6 +52,15 @@ public LaunchRequest getLaunchRequest() { private void refreshRequest() { currentRequest = currentCalculator.createLaunchRequest(); + SmartDashboard.putNumber( + "Current Launch Request/Target Robot Angle (degrees)", + currentRequest.getTargetRobotAngle().getDegrees()); + SmartDashboard.putNumber( + "Current Launch Request/Target Flywheel Velocity (rps)", + currentRequest.getFlywheelVelocity().in(RotationsPerSecond)); + SmartDashboard.putNumber( + "Current Launch Request/Target Hood Angle (rotations)", + currentRequest.getHoodTarget().in(Rotations)); } public enum LaunchType { diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java index 45371e7..21c135b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -24,7 +24,7 @@ public LaunchRequest( double timestamp) { this.launchHoodTarget = Rotations.of( - launchHoodTarget.in(Radians) + launchHoodTarget.in(Degrees) * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in( Rotations)); // converts froms radians to rotations this.launchVelocity = diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index e0030d1..0c87404 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -28,7 +28,7 @@ public LaunchRequest createLaunchRequest( if (passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; else slope = ShooterConstants.OPTIMAL_HUB_ENTRY_SLOPE; - double a, b, vertex; + double a, b, vertex, hitWallCheckX; Angle theta, motorAngle; do { // system of equations @@ -39,8 +39,9 @@ public LaunchRequest createLaunchRequest( theta = Radians.of(Math.atan(b)); // launch angle (Hood Angle Conversion: MATH.PI/2 - theta) motorAngle = Radians.of(Math.PI / 2 - theta.in(Radians)); vertex = -1 * b / (2 * a); + hitWallCheckX = x2 - ShooterConstants.FROM_HUB_CENTER_TO_WALL.in(Meters); slope -= 0.05; - } while ((passing || vertex > x2 - ShooterConstants.MIN_HUB_VERTEX_DISTANCE.in(Meters)) + } while ((passing || a*Math.pow(hitWallCheckX,2) + b*hitWallCheckX + y1 > ShooterConstants.HUB_HEIGHT.in(Meters)) && !(motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees))); if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index d51d2a2..b92d3e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -68,9 +68,9 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() // Lemon Yeeting Constants public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate - public static final Distance GOAL_HEIGHT = Inch.of(23); // TODO: Get Better Estimate + public static final Distance HUB_HEIGHT = Inch.of(23); // TODO: Get Better Estimate - public static final Distance MIN_HUB_VERTEX_DISTANCE = Inch.of(23.5); + public static final Distance FROM_HUB_CENTER_TO_WALL = Inch.of(23.5); public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate public static final double OPTIMAL_PASSING_ENTRY_SLOPE = -1; // TODO: Tune public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; // TODO: Tune From f07fdbef6d3c6a1afd458f3ed30568127b250e35 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Thu, 26 Feb 2026 21:13:37 -0500 Subject: [PATCH 8/9] :P --- .../subsystems/shooter/ParabolicLaunchRequestBuilder.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index 0c87404..a7e4418 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -22,7 +22,7 @@ public LaunchRequest createLaunchRequest( Rotation2d targetRobotAngle) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); double x2 = distance; - double y2 = passing ? 0 : ShooterConstants.GOAL_HEIGHT.in(Meters); + double y2 = passing ? 0 : ShooterConstants.HUB_HEIGHT.in(Meters); double slope; if (passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; @@ -41,7 +41,9 @@ public LaunchRequest createLaunchRequest( vertex = -1 * b / (2 * a); hitWallCheckX = x2 - ShooterConstants.FROM_HUB_CENTER_TO_WALL.in(Meters); slope -= 0.05; - } while ((passing || a*Math.pow(hitWallCheckX,2) + b*hitWallCheckX + y1 > ShooterConstants.HUB_HEIGHT.in(Meters)) + } while ((passing + || a * Math.pow(hitWallCheckX, 2) + b * hitWallCheckX + y1 + > ShooterConstants.HUB_HEIGHT.in(Meters)) && !(motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees))); if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null; From ae1c2c1bfc2bfed51bdcafc208bc340c30e13131 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 28 Feb 2026 16:02:15 -0500 Subject: [PATCH 9/9] Buildable --- src/main/java/frc/robot/RobotContainer.java | 29 ++++------- .../subsystems/indexer/IndexerConstants.java | 20 +++++++- .../subsystems/indexer/IndexerSubsystem.java | 26 ++++++++-- ...references.java => IntakePreferences.java} | 3 ++ .../subsystems/shooter/ShooterConstants.java | 51 +++++++++++++++++-- .../shooter/ShooterPreferences.java | 3 ++ .../subsystems/shooter/ShooterSubsystem.java | 39 ++++++++++++-- 7 files changed, 139 insertions(+), 32 deletions(-) rename src/main/java/frc/robot/subsystems/indexer/{IndexerPreferences.java => IntakePreferences.java} (67%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d5719e..a815a03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,19 +4,19 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.statemachines.LaunchState; -import frc.robot.statemachines.LaunchState.LaunchType; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DrivetrainSubsystem; @@ -28,7 +28,8 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); - private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController driverJoystick = new CommandXboxController(0); + private final CommandXboxController operatorJoystick = new CommandXboxController(1); public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); @@ -60,7 +61,6 @@ public RobotContainer() { } public void configureSubsystemDefaultCommands() { - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest( @@ -68,23 +68,25 @@ public void configureSubsystemDefaultCommands() { DriveConstants.DEFAULT_DRIVE_REQUEST .withVelocityX( -1 - * Math.copySign(Math.pow(joystick.getLeftY(), 2), joystick.getLeftY()) + * Math.copySign( + Math.pow(driverJoystick.getLeftY(), 2), driverJoystick.getLeftY()) * DriveConstants .MAX_DRIVE_SPEED) // Drive forward with negative Y (forward) .withVelocityY( -1 - * Math.copySign(Math.pow(joystick.getLeftX(), 2), joystick.getLeftX()) + * Math.copySign( + Math.pow(driverJoystick.getLeftX(), 2), driverJoystick.getLeftX()) * DriveConstants.MAX_DRIVE_SPEED) // Drive left with negative X (left) .withRotationalRate( -1 - * Math.copySign(Math.pow(joystick.getRightX(), 2), joystick.getRightX()) + * Math.copySign( + Math.pow(driverJoystick.getRightX(), 2), driverJoystick.getRightX()) * DriveConstants .MAX_ANGULAR_SPEED) // Drive counterclockwise with negative X (left) .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1) .withRotationalDeadband(DriveConstants.MAX_ANGULAR_SPEED * 0.1))); } - public void removeSubsystemDefaultCommands() { drivetrain.removeDefaultCommand(); } @@ -156,15 +158,6 @@ public void configureTeleopBindings() { driverJoystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.registerTelemetry(logger::telemeterize); - */ - - joystick - .x() - .onTrue( - Commands.runOnce( - () -> - launchState.activateCalculator( - new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), LaunchType.PARABOLIC))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 6ad3636..782b656 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -1,13 +1,17 @@ package frc.robot.subsystems.indexer; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; final class IndexerConstants { private IndexerConstants() {} // TODO Replace with real id - protected static final int INDEXER_MOTOR_ID = 6; + protected static final int INDEXER_MOTOR_LEADER_ID = 3; + protected static final int INDEXER_MOTOR_FOLLOWER_ID = 9; // TODO: Tune motor @@ -24,4 +28,18 @@ protected static Slot0Configs createIndexerMotorSlot0Configs() { slot.kD = INDEXER_KD; return slot; } + + public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } + + public static MotorOutputConfigs createFollowerMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 9bee08d..0f28c9e 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -4,8 +4,10 @@ import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.AngularVelocity; @@ -15,7 +17,8 @@ @Logged public class IndexerSubsystem extends SubsystemBase { - private final TalonFX indexerMotor; + private final TalonFX indexerMotorLeader; + private final TalonFX indexerMotorFollower; @Logged(name = "Indexer Velocity Target", importance = Importance.CRITICAL) private AngularVelocity indexerVelocityTarget; // RotationsPerSecond @@ -33,16 +36,29 @@ public class IndexerSubsystem extends SubsystemBase { new SysIdRoutine.Mechanism(output -> setIndexerVoltage(output.magnitude()), null, this)); public IndexerSubsystem() { - indexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR_ID); - indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + indexerMotorLeader = new TalonFX(IndexerConstants.INDEXER_MOTOR_LEADER_ID); + indexerMotorFollower = new TalonFX(IndexerConstants.INDEXER_MOTOR_FOLLOWER_ID); + + indexerMotorLeader.getConfigurator().apply(IndexerConstants.createLeaderMotorOutputConfigs()); + indexerMotorFollower + .getConfigurator() + .apply(IndexerConstants.createFollowerMotorOutputConfigs()); + + indexerMotorLeader.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + indexerMotorFollower.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + + indexerMotorFollower.setControl( + new Follower(indexerMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); + indexerVelocityTarget = RotationsPerSecond.of(0); indexerControl = new VelocityVoltage(0); } @Override public void periodic() { - indexerMotor.setControl( - indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); + // TODO: removed for testing only. PUT IT BACK! + // indexerMotor.setControl( + // indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); } private void setIndexerVoltage(double magnitude) { diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IntakePreferences.java similarity index 67% rename from src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java rename to src/main/java/frc/robot/subsystems/indexer/IntakePreferences.java index 9e67f54..326f728 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java +++ b/src/main/java/frc/robot/subsystems/indexer/IntakePreferences.java @@ -8,4 +8,7 @@ private IndexerPreferences() {} protected static DoublePreference indexSpeed = new DoublePreference("Indexer/Index Speed", 0.1); // in rotations per second + + public static DoublePreference indexerPercent = + new DoublePreference("Indexer/Index Percent (for without PID)", 0.1); // in percent } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index e2ac9c0..fb7d3ef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,9 +2,14 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; @@ -13,9 +18,23 @@ public class ShooterConstants { private ShooterConstants() {} - // TODO: Change to actual ports - public static final int FLYWHEEL_MOTOR_ID = 4; - public static final int HOOD_MOTOR_ID = 5; + public static final int FLYWHEEL_LEADER_MOTOR_ID = 5; + public static final int FLYWHEEL_FOLLOWER_MOTOR_ID = 6; + public static final int HOOD_MOTOR_ID = 4; + + public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } + + public static MotorOutputConfigs createFollowerMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } // TODO: Tune Flywheel and Hood Motor @@ -35,7 +54,7 @@ public static Slot0Configs createFlywheelMotorSlot0Configs() { } public static final double ALLOWABLE_HOOD_ERROR = 0.1; - public static final double HOOD_FORWARD_LIMIT = 100; + public static final double HOOD_FORWARD_LIMIT = 5.8; public static final double HOOD_REVERSE_LIMIT = 0; public static final double HOOD_KS = 0; public static final double HOOD_KP = 0; @@ -58,6 +77,13 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() return configs; } + public static MotorOutputConfigs createHoodMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } + public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); @@ -69,9 +95,24 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() // Lemon Yeeting Constants public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate public static final Distance HUB_HEIGHT = Inch.of(23); // TODO: Get Better Estimate - public static final Distance FROM_HUB_CENTER_TO_WALL = Inch.of(23.5); + public static final Distance MIN_VERTEX_DISTANCE = Inch.of(23.5); public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate public static final double OPTIMAL_PASSING_ENTRY_SLOPE = -1; // TODO: Tune public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; // TODO: Tune + + // TODO: verify these! + // funnel poses. + public static final Pose3d BLUE_TARGET = + new Pose3d( + Distance.ofBaseUnits(4.623, Meters), + Distance.ofBaseUnits(4.041, Meters), + Distance.ofBaseUnits(1.435, Meters), + Rotation3d.kZero); + public static final Pose3d RED_TARGET = + new Pose3d( + Distance.ofBaseUnits(12.276, Meters), + Distance.ofBaseUnits(4.041, Meters), + Distance.ofBaseUnits(1.435, Meters), + Rotation3d.kZero); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java index 0a8c399..14d947b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java @@ -10,4 +10,7 @@ private ShooterPreferences() {} new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second public static DoublePreference hoodLaunchAngle = new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations + + public static DoublePreference flywheelLaunchPercent = + new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 1b4f60c..efc8ca0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -21,7 +21,8 @@ @Logged public class ShooterSubsystem extends SubsystemBase { - private final TalonFX flywheelMotor; + private final TalonFX flywheelMotorLeader; + private final TalonFX flywheelMotorFollower; private final TalonFX hoodMotor; @Logged(name = "Velocity Target", importance = Importance.CRITICAL) @@ -47,7 +48,8 @@ public class ShooterSubsystem extends SubsystemBase { new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); public ShooterSubsystem() { - flywheelMotor = new TalonFX(ShooterConstants.FLYWHEEL_MOTOR_ID); + flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); + flywheelMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_FOLLOWER_MOTOR_ID); hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); @@ -96,7 +98,8 @@ public Command stopFlywheelCommand() { } private void setFlywheelVoltage(double magnitude) { - flywheelMotor.setVoltage(magnitude); + flywheelMotorLeader.setVoltage(magnitude); + flywheelMotorFollower.setVoltage(magnitude); } @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) @@ -117,6 +120,28 @@ public Command launchLemonsCommand() { .withName("Start Launching Lemons"); } + public Command launchLemonsCommandNoPID() { + return setHoodCommand(Rotations.of(ShooterPreferences.hoodLaunchAngle.getValue())) + .andThen( + runOnce( + () -> { + flywheelMotorLeader.set(ShooterPreferences.flywheelLaunchPercent.getValue()); + flywheelMotorFollower.set(ShooterPreferences.flywheelLaunchPercent.getValue()); + })) + .withName("Start Launching Lemons (No PID)"); + } + + public Command stopLaunchLemonsNoPIDCommand() { + return setHoodCommand(Rotations.of(0)) + .andThen( + runOnce( + () -> { + flywheelMotorLeader.set(0); + flywheelMotorFollower.set(0); + })) + .withName("Stop Launching Lemons (No PID)"); + } + public Command stowCommand() { return stopFlywheelCommand().andThen(setHoodCommand(Rotations.of(0))).withName("Stow Shooter"); } @@ -131,4 +156,12 @@ public Command homeShooterCommand() { > ShooterConstants.SAFE_STATOR_LIMIT.in(Amp); }); } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineFlywheel.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineFlywheel.dynamic(direction); + } }