diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 86c71853b..bbf5f5111 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -8,11 +8,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; @@ -28,6 +28,9 @@ import org.littletonrobotics.junction.Logger; public class Orchestrator { + 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; private final Shooter shooter; @@ -158,13 +161,11 @@ 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))) - .withName("spinUpShooterTest"); +<<<<<<< HEAD + return shooter.setTargetVelocityRadians(() -> testRPM.get()).withName("spinUpShooterTest"); +======= + return shooter.setTargetVelocityRadians(testRadPerSec).withName("spinUpShooterTest"); +>>>>>>> refs/remotes/origin/regression } 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;