From 5ee90f025d453a1e737950f6c3b1f0bc97b5625c Mon Sep 17 00:00:00 2001 From: William Crouch Date: Tue, 21 Apr 2026 20:47:46 -0400 Subject: [PATCH 1/5] We need to think about the PID formula --- .../robot/statemachines/LaunchCalculator.java | 22 ++----------------- .../subsystems/drive/DriveConstants.java | 2 ++ .../subsystems/drive/DrivePreferences.java | 3 +++ 3 files changed, 7 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 2ac1a37..93d2fd1 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -14,6 +14,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.statemachines.LaunchState.LaunchType; +import frc.robot.subsystems.drive.DrivePreferences; import frc.robot.subsystems.shooter.LaunchRequest; import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; @@ -32,7 +33,7 @@ protected static synchronized LaunchCalculator getInstance() { private double loopPeriodSecs = 0.02; private final LinearFilter driveAngleFilter = - LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs)); + LinearFilter.movingAverage(DrivePreferences.autoAimFilterTaps.getValue()); private static final double phaseDelay; @@ -115,9 +116,6 @@ protected LaunchRequest refreshRequest(Pose3d target, LaunchType builderType) { Rotation2d targetRobotAngle = target.getTranslation().toTranslation2d().minus(lookaheadPose.getTranslation()).getAngle(); - // Rotation2d targetRobotAngle = getDriveAngle(lookaheadPose, - // target.getTranslation().toTranslation2d()); - AngularVelocity targetRobotAngularVelocity = // RadiansPerSecond.of( // driveAngleFilter.calculate( @@ -148,20 +146,4 @@ protected LaunchRequest refreshRequest(Pose3d target, LaunchType builderType) { targetRobotAngle, Meters.of(launcherToTargetDistance)); } - - /* - 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/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5c2abcc..0f8adba 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -44,4 +44,6 @@ public class DriveConstants { public static final double SPIN_MOVE_PIVOT_CLEARANCE_METERS = 0.05; public static final double HUNT_SPEED = 1.5; //meters/s? + + public static final int LINEAR_FILTER_AUTO_AIM_TAPS = 5; } diff --git a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java index dbdc699..30b57d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drive; import frc.robot.preferences.DoublePreference; +import frc.robot.preferences.IntegerPreference; public final class DrivePreferences { @@ -20,4 +21,6 @@ public final class DrivePreferences { public static DoublePreference spinMoveAngularSpeed = new DoublePreference("Drive/SpinMove/AngularSpeed", DriveConstants.MAX_ANGULAR_SPEED); + + public static IntegerPreference autoAimFilterTaps = new IntegerPreference("Drive/AutoAim/FilterTaps", DriveConstants.LINEAR_FILTER_AUTO_AIM_TAPS); } From 123b3e32cc016f6b33f8c0a7e047f7ab16ba6f0b Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 22 Apr 2026 20:18:47 -0400 Subject: [PATCH 2/5] Redid PID formula --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++-------- .../robot/statemachines/LaunchCalculator.java | 20 ---------------- .../subsystems/shooter/LaunchRequest.java | 9 ------- .../shooter/LaunchRequestBuilder.java | 1 - .../shooter/MappedLaunchRequestBuilder.java | 2 -- .../ParabolicLaunchRequestBuilder.java | 2 -- 6 files changed, 14 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 470c5a4..e8c0608 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -308,18 +309,21 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } + PIDController controller = new PIDController(0,0,0); + private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate = - launchRequest.getTargetRobotAngularVelocity().in(RadiansPerSecond) - + DrivePreferences.autoAim_kP.getValue() + DrivePreferences.autoAim_kP.getValue() * launchRequest .getTargetRobotAngle() .minus(driveState.getCurrentDriveStats().Pose.getRotation()) .getRadians() - + DrivePreferences.autoAim_kD.getValue() - * (launchRequest.getTargetRobotAngularVelocity().in(RadiansPerSecond) - - driveState.getFieldVelocity().omegaRadiansPerSecond); + + DrivePreferences.autoAim_kD.getValue() + * driveState.getPreviousDriveStats().Pose.getRotation() + .minus(driveState.getCurrentDriveStats().Pose.getRotation()) + .getRadians() / (driveState.getCurrentDriveStats().Timestamp - driveState.getPreviousDriveStats().Timestamp); + return DriveConstants.DEFAULT_DRIVE_REQUEST .withVelocityX( -1 @@ -336,14 +340,14 @@ private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { private SwerveRequest.RobotCentric getDriveToLemonRequest() { Pose2d clusterPose = hunter.getClusterCentroid(driveState.getCurrentDriveStats().Pose); Rotation2d targetAngle = clusterPose.getTranslation().minus(driveState.getCurrentDriveStats().Pose.getTranslation()).getAngle(); - AngularVelocity targetAngularVelocity = RadiansPerSecond.of(targetAngle.minus(driveState.getPreviousDriveStats().Pose.getRotation()).getRadians()); - + double rotationalRate = - targetAngularVelocity.in(RadiansPerSecond) - + DrivePreferences.autoAim_kP.getValue() + DrivePreferences.autoAim_kP.getValue() * targetAngle.minus(driveState.getCurrentDriveStats().Pose.getRotation()).getRadians() + DrivePreferences.autoAim_kD.getValue() - * (targetAngularVelocity.in(RadiansPerSecond) - driveState.getFieldVelocity().omegaRadiansPerSecond); + * driveState.getPreviousDriveStats().Pose.getRotation() + .minus(driveState.getCurrentDriveStats().Pose.getRotation()) + .getRadians() / (driveState.getCurrentDriveStats().Timestamp - driveState.getPreviousDriveStats().Timestamp); return DriveConstants.LEMON_HUNTING_REQUEST .withVelocityX(DriveConstants.HUNT_SPEED) // Drive forward with negative Y (forward) diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 93d2fd1..48c7b39 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -30,11 +30,6 @@ protected static synchronized LaunchCalculator getInstance() { return single_instance; } - private double loopPeriodSecs = 0.02; - - private final LinearFilter driveAngleFilter = - LinearFilter.movingAverage(DrivePreferences.autoAimFilterTaps.getValue()); - private static final double phaseDelay; private static final InterpolatingDoubleTreeMap hubTimeOfFlightMap = @@ -116,25 +111,11 @@ protected LaunchRequest refreshRequest(Pose3d target, LaunchType builderType) { Rotation2d targetRobotAngle = target.getTranslation().toTranslation2d().minus(lookaheadPose.getTranslation()).getAngle(); - AngularVelocity targetRobotAngularVelocity = - // RadiansPerSecond.of( - // driveAngleFilter.calculate( - // targetRobotAngle - // - // .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()) - // .getRadians() - // / loopPeriodSecs)); - RadiansPerSecond.of( - targetRobotAngle - .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()) - .getRadians()); - if (builderType == LaunchType.MAPPED) return new MappedLaunchRequestBuilder() .createLaunchRequest( passing, lookaheadLauncherToTargetDistance, - targetRobotAngularVelocity, targetRobotAngle, Meters.of(launcherToTargetDistance)); else @@ -142,7 +123,6 @@ protected LaunchRequest refreshRequest(Pose3d target, LaunchType builderType) { .createLaunchRequest( passing, lookaheadLauncherToTargetDistance, - targetRobotAngularVelocity, targetRobotAngle, Meters.of(launcherToTargetDistance)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java index 4ba8975..e9bf278 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java @@ -14,9 +14,6 @@ public class LaunchRequest { @Logged(name = "Launch Flywheel Velocity", importance = Logged.Importance.CRITICAL) private AngularVelocity launchVelocity; - @Logged(name = "Target Robot Angular Velocity", importance = Logged.Importance.CRITICAL) - private AngularVelocity targetRobotAngularVelocity; - @Logged(name = "Target Robot Angle", importance = Logged.Importance.CRITICAL) private Rotation2d targetRobotAngle; @@ -28,13 +25,11 @@ public class LaunchRequest { public LaunchRequest( Angle launchHoodTarget, AngularVelocity launchVelocity, - AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle, Distance targetDistance, double timestamp) { this.launchHoodTarget = launchHoodTarget; this.launchVelocity = launchVelocity; - this.targetRobotAngularVelocity = targetRobotAngularVelocity; this.targetRobotAngle = targetRobotAngle; this.targetDistance = targetDistance; this.timestamp = timestamp; @@ -48,10 +43,6 @@ public AngularVelocity getFlywheelVelocity() { return launchVelocity; } - public AngularVelocity getTargetRobotAngularVelocity() { - return targetRobotAngularVelocity; - } - public Rotation2d getTargetRobotAngle() { return targetRobotAngle; } diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java index f9c4ded..a4ffc4e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -8,7 +8,6 @@ public interface LaunchRequestBuilder { public LaunchRequest createLaunchRequest( boolean passing, double distance, - AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle, Distance targetDistance); } diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index b53dac0..ec419e1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -83,7 +83,6 @@ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { public LaunchRequest createLaunchRequest( boolean passing, double distance, - AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle, Distance targetDistance) { @@ -101,7 +100,6 @@ public LaunchRequest createLaunchRequest( return new LaunchRequest( Rotations.of(hoodAngle), RotationsPerSecond.of(flywheelSpeed), - targetRobotAngularVelocity, targetRobotAngle, targetDistance, 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 b1f2304..01df21d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -19,7 +19,6 @@ public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder { public LaunchRequest createLaunchRequest( boolean passing, double distance, - AngularVelocity targetRobotAngularVelocity, Rotation2d targetRobotAngle, Distance targetDistance) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); @@ -77,7 +76,6 @@ public LaunchRequest createLaunchRequest( return new LaunchRequest( motorAngle, angularVelocity, - targetRobotAngularVelocity, targetRobotAngle, targetDistance, Utils.getCurrentTimeSeconds()); From 3dcaa80d9df6875c95be421070da0f1e1d06867b Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 22 Apr 2026 20:26:47 -0400 Subject: [PATCH 3/5] Explanation --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8c0608..5fa7828 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -314,11 +314,13 @@ public Command getAutonomousCommand() { private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate = + //kP * error DrivePreferences.autoAim_kP.getValue() * launchRequest .getTargetRobotAngle() .minus(driveState.getCurrentDriveStats().Pose.getRotation()) .getRadians() + //kD * rate of error ((target - currentAngle - (target - previousAngle))/period = (previousAngle - currentAngle)/period) + DrivePreferences.autoAim_kD.getValue() * driveState.getPreviousDriveStats().Pose.getRotation() .minus(driveState.getCurrentDriveStats().Pose.getRotation()) From 49b322ff690526198c7419a895afa531b2713297 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 22 Apr 2026 20:31:16 -0400 Subject: [PATCH 4/5] Cleanup --- src/main/java/frc/robot/RobotContainer.java | 48 ++++++++++++------- .../robot/statemachines/LaunchCalculator.java | 4 -- .../subsystems/drive/DriveConstants.java | 2 +- .../subsystems/drive/DrivePreferences.java | 3 -- .../shooter/LaunchRequestBuilder.java | 6 +-- .../shooter/MappedLaunchRequestBuilder.java | 6 +-- .../ParabolicLaunchRequestBuilder.java | 5 +- 7 files changed, 35 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26fa8a0..3137c7f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,6 @@ package frc.robot; -import static edu.wpi.first.units.Units.RadiansPerSecond; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -14,7 +12,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -308,23 +305,29 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - PIDController controller = new PIDController(0,0,0); + PIDController controller = new PIDController(0, 0, 0); private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate = - //kP * error + // kP * error DrivePreferences.autoAim_kP.getValue() * launchRequest .getTargetRobotAngle() .minus(driveState.getCurrentDriveStats().Pose.getRotation()) .getRadians() - //kD * rate of error ((target - currentAngle - (target - previousAngle))/period = (previousAngle - currentAngle)/period) - + DrivePreferences.autoAim_kD.getValue() - * driveState.getPreviousDriveStats().Pose.getRotation() - .minus(driveState.getCurrentDriveStats().Pose.getRotation()) - .getRadians() / (driveState.getCurrentDriveStats().Timestamp - driveState.getPreviousDriveStats().Timestamp); - + // kD * rate of error ((target - currentAngle - (target - previousAngle))/period = + // (previousAngle - currentAngle)/period) + + DrivePreferences.autoAim_kD.getValue() + * driveState + .getPreviousDriveStats() + .Pose + .getRotation() + .minus(driveState.getCurrentDriveStats().Pose.getRotation()) + .getRadians() + / (driveState.getCurrentDriveStats().Timestamp + - driveState.getPreviousDriveStats().Timestamp); + return DriveConstants.DEFAULT_DRIVE_REQUEST .withVelocityX( -1 @@ -340,15 +343,26 @@ private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { private SwerveRequest.RobotCentric getDriveToLemonRequest() { Pose2d clusterPose = hunter.getClusterCentroid(driveState.getCurrentDriveStats().Pose); - Rotation2d targetAngle = clusterPose.getTranslation().minus(driveState.getCurrentDriveStats().Pose.getTranslation()).getAngle(); + Rotation2d targetAngle = + clusterPose + .getTranslation() + .minus(driveState.getCurrentDriveStats().Pose.getTranslation()) + .getAngle(); double rotationalRate = DrivePreferences.autoAim_kP.getValue() - * targetAngle.minus(driveState.getCurrentDriveStats().Pose.getRotation()).getRadians() - + DrivePreferences.autoAim_kD.getValue() - * driveState.getPreviousDriveStats().Pose.getRotation() - .minus(driveState.getCurrentDriveStats().Pose.getRotation()) - .getRadians() / (driveState.getCurrentDriveStats().Timestamp - driveState.getPreviousDriveStats().Timestamp); + * targetAngle + .minus(driveState.getCurrentDriveStats().Pose.getRotation()) + .getRadians() + + DrivePreferences.autoAim_kD.getValue() + * driveState + .getPreviousDriveStats() + .Pose + .getRotation() + .minus(driveState.getCurrentDriveStats().Pose.getRotation()) + .getRadians() + / (driveState.getCurrentDriveStats().Timestamp + - driveState.getPreviousDriveStats().Timestamp); return DriveConstants.LEMON_HUNTING_REQUEST .withVelocityX(DriveConstants.HUNT_SPEED) // Drive forward with negative Y (forward) diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java index 48c7b39..fcd2152 100644 --- a/src/main/java/frc/robot/statemachines/LaunchCalculator.java +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -1,9 +1,7 @@ package frc.robot.statemachines; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RadiansPerSecond; -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; @@ -11,10 +9,8 @@ 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.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.statemachines.LaunchState.LaunchType; -import frc.robot.subsystems.drive.DrivePreferences; import frc.robot.subsystems.shooter.LaunchRequest; import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 16b895e..a28bc9e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -38,5 +38,5 @@ public class DriveConstants { public static final double SPIN_MOVE_PIVOT_CLEARANCE_METERS = 0.05; - public static final double HUNT_SPEED = 1.5; //meters/s? + public static final double HUNT_SPEED = 1.5; // meters/s? } diff --git a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java index 30b57d3..dbdc699 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivePreferences.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.drive; import frc.robot.preferences.DoublePreference; -import frc.robot.preferences.IntegerPreference; public final class DrivePreferences { @@ -21,6 +20,4 @@ public final class DrivePreferences { public static DoublePreference spinMoveAngularSpeed = new DoublePreference("Drive/SpinMove/AngularSpeed", DriveConstants.MAX_ANGULAR_SPEED); - - public static IntegerPreference autoAimFilterTaps = new IntegerPreference("Drive/AutoAim/FilterTaps", DriveConstants.LINEAR_FILTER_AUTO_AIM_TAPS); } diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java index a4ffc4e..abb9db4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -1,13 +1,9 @@ package frc.robot.subsystems.shooter; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; public interface LaunchRequestBuilder { public LaunchRequest createLaunchRequest( - boolean passing, - double distance, - Rotation2d targetRobotAngle, - Distance targetDistance); + boolean passing, double distance, Rotation2d targetRobotAngle, Distance targetDistance); } diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index ec419e1..5095939 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -10,7 +10,6 @@ import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; /** Add your docs here. */ @@ -81,10 +80,7 @@ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { } public LaunchRequest createLaunchRequest( - boolean passing, - double distance, - Rotation2d targetRobotAngle, - Distance targetDistance) { + boolean passing, double distance, Rotation2d targetRobotAngle, Distance targetDistance) { double hoodAngle, flywheelSpeed; if (passing) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index 01df21d..996afbd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -17,10 +17,7 @@ public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder { public LaunchRequest createLaunchRequest( - boolean passing, - double distance, - Rotation2d targetRobotAngle, - Distance targetDistance) { + boolean passing, double distance, Rotation2d targetRobotAngle, Distance targetDistance) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); double x2 = distance; double y2 = passing ? 0 : ShooterConstants.HUB_HEIGHT.in(Meters); From d8c80505cceac4fb28346328048316b338f4e22a Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 22 Apr 2026 20:37:11 -0400 Subject: [PATCH 5/5] More cleanup --- src/main/java/frc/robot/RobotContainer.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3137c7f..1a91da4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -305,8 +304,6 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - PIDController controller = new PIDController(0, 0, 0); - private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate =