From a992af727d7fd1283c44d3d00b378caaeaa73696 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Wed, 1 Apr 2026 19:06:29 -0400 Subject: [PATCH 1/4] fix regression --- src/main/java/frc/robot/Orchestrator.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 881435932..f2d62863f 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -7,11 +7,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.util.Units; -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 frc.lib.utils.AllianceFlipUtil; +import frc.lib.utils.TunableNumber; import frc.robot.FieldConstants.Hub; import frc.robot.commands.auto.DriveToPose; import frc.robot.commands.auto.RotateToOrientation; @@ -27,6 +27,9 @@ import org.littletonrobotics.junction.Logger; public class Orchestrator { + private static final TunableNumber testRPM = + new TunableNumber("Shooter/TestRPM", CLOSE_HUB_SHOOTER_RPM); + private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0); private final Drive drive; private final Shooter shooter; @@ -152,12 +155,8 @@ public Command feedUp() { } public Command spinUpShooterTest() { - SmartDashboard.putNumber("Shooter/TestRPM", CLOSE_HUB_SHOOTER_RPM); return shooter - .setTargetVelocityRadians( - () -> - Units.rotationsPerMinuteToRadiansPerSecond( - SmartDashboard.getNumber("Shooter/TestRPM", 1000.0))) + .setTargetVelocityRadians(() -> Units.rotationsPerMinuteToRadiansPerSecond(testRPM.get())) .withName("spinUpShooterTest"); } From d1ef6f4ccc6d0cc487e5922b6046fb2bd4a91838 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Wed, 1 Apr 2026 19:13:12 -0400 Subject: [PATCH 2/4] turn on tuning --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 922aca595..d8b03fb6d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,7 +10,7 @@ public class Constants { // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT private static final RobotType ROBOT_TYPE_OVERRIDE = null; - public static final boolean tuningMode = false; + public static final boolean tuningMode = true; private static final String ROBOT_FILENAME = "/home/lvuser/robot"; private static RobotType cachedRobotTypeFromRoborio = null; From 1140ff185888fee4d3f7a9bb93cf2dc5159b942c Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Wed, 1 Apr 2026 19:30:32 -0400 Subject: [PATCH 3/4] fix to radians --- src/main/java/frc/robot/Orchestrator.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 3f71a9b41..3155b79ed 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -28,8 +28,8 @@ import org.littletonrobotics.junction.Logger; public class Orchestrator { - private static final TunableNumber testRPM = - new TunableNumber("Shooter/TestRPM", CLOSE_HUB_SHOOTER_RPM); + private static final TunableNumber testRadPerSec = + new TunableNumber("Shooter/TestRadPerSec", CLOSE_HUB_SHOOTER_RPM); private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0); private final Drive drive; @@ -161,9 +161,7 @@ public Command feedUp() { } public Command spinUpShooterTest() { - return shooter - .setTargetVelocityRadians(() -> Units.rotationsPerMinuteToRadiansPerSecond(testRPM.get())) - .withName("spinUpShooterTest"); + return shooter.setTargetVelocityRadians(testRadPerSec).withName("spinUpShooterTest"); } public Command spinUpShooterDistance(DoubleSupplier targetDistance) { From 274d9914cf4d660c36cc26fea4c71644bf21b60e Mon Sep 17 00:00:00 2001 From: Driver Station Date: Wed, 1 Apr 2026 20:17:21 -0400 Subject: [PATCH 4/4] Adding regression changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Orchestrator.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/commands/auto/Autos.java | 40 ------------------- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- 5 files changed, 5 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d8b03fb6d..922aca595 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,7 +10,7 @@ public class Constants { // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT private static final RobotType ROBOT_TYPE_OVERRIDE = null; - public static final boolean tuningMode = true; + public static final boolean tuningMode = false; private static final String ROBOT_FILENAME = "/home/lvuser/robot"; private static RobotType cachedRobotTypeFromRoborio = null; diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 3f71a9b41..40bc9f465 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -161,9 +161,7 @@ public Command feedUp() { } public Command spinUpShooterTest() { - return shooter - .setTargetVelocityRadians(() -> Units.rotationsPerMinuteToRadiansPerSecond(testRPM.get())) - .withName("spinUpShooterTest"); + return shooter.setTargetVelocityRadians(() -> testRPM.get()).withName("spinUpShooterTest"); } public Command spinUpShooterDistance(DoubleSupplier targetDistance) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bf5f4c1ba..50bd24b44 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -308,8 +308,8 @@ public RobotContainer() { autoChooser.addOption("pp", autos.ppACycleLeft()); autoChooser.addOption("ppB", autos.ppBCycleRight()); - autoChooser.addOption("ppB Cycle Right Regression", autos.ppBCycleRightRegression()); - autoChooser.addOption("ppA Cycle Left Regression", autos.ppACycleLeftRegression()); + // autoChooser.addOption("ppB Cycle Right Regression", autos.ppBCycleRightRegression()); + // autoChooser.addOption("ppA Cycle Left Regression", autos.ppACycleLeftRegression()); // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 89dfc9a62..225696ea6 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -149,46 +149,6 @@ public Command ppBCycleRight() { orchestrator.feedUp())); } - public Command ppBCycleRightRegression() { - return Commands.sequence( - Commands.runOnce(() -> drive.setPose(startBside.get())), - Commands.deadline( - drive.getAutonomousCommand("B-Cycle-RightSweep"), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) - .withTimeout(14.5), - Commands.deadline( - orchestrator.aimToHub().withTimeout(2.5), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), - Commands.parallel( - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp()) - .withTimeout(2.5), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp())); - } - - public Command ppACycleLeftRegression() { - return Commands.sequence( - Commands.runOnce(() -> drive.setPose(startAside.get())), - Commands.deadline( - drive.getAutonomousCommand("A-Cycle-LeftSweep"), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) - .withTimeout(14.5), - Commands.deadline( - orchestrator.aimToHub().withTimeout(2.5), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), - Commands.parallel( - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp()) - .withTimeout(2.5), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp())); - } - public Command EightBalls() { return Commands.sequence( Commands.deadline( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b9aac7cf9..b906761fd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -174,7 +174,7 @@ public double getAirTimeSeconds(DoubleSupplier distance) { public DoubleSupplier calculateSetpoint(DoubleSupplier distance) { // calculate rad/s depending on distance return () -> { - double setpoint = 183.35 * distance.getAsDouble() + 750.93; + double setpoint = 223 * distance.getAsDouble() + 750.93; if (setpoint > 5000) { return 5000; } else return setpoint;