diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 687a4ad..4c1f485 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.Controller; import frc.robot.Robot.ControlMode; -import frc.robot.commands.ShootSpeakerPoselessCommand; +import frc.robot.commands.TimedSpeakerShotCommand; import frc.robot.commands.TeleopSwerveCommand; import frc.robot.subsystems.*; import frc.robot.util.Gamepad; @@ -28,7 +28,7 @@ public class RobotContainer { public final LinearActuatorSubsystem LinearActuator = new LinearActuatorSubsystem(); public final PoseEstimationSubsystem PoseEstimation = new PoseEstimationSubsystem(Swerve::getYaw, Swerve::getPositions); - private final ShootSpeakerPoselessCommand ShootSpeakerPoselessCMD = new ShootSpeakerPoselessCommand(Shooter); + private final TimedSpeakerShotCommand TimedSpeakerShot = new TimedSpeakerShotCommand(Shooter); Gamepad DRIVER = new Gamepad(Controller.DRIVER_PORT); Gamepad OPERATOR = new Gamepad(Controller.OPERATOR_PORT); @@ -90,7 +90,7 @@ private void configureDefaultCommands() { Shooter.setDefaultCommand( new RunCommand( - () -> Shooter.flywheelSpeaker(OPERATOR.getXButton()), + () -> Shooter.flywheelSpeaker(false), Shooter)); LinearActuator.setDefaultCommand( @@ -101,7 +101,7 @@ private void configureDefaultCommands() { ); } - private void configureButtonBindings() { // MOVE POOP FUNCTION TO ANOTHER BUTTON + private void configureButtonBindings() { new JoystickButton(OPERATOR, 2) .whileTrue( new RunCommand(() -> Shooter.intake(true, true), Shooter)); @@ -124,7 +124,7 @@ private void configureButtonBindings() { // MOVE POOP FUNCTION TO ANOTHER BUTTON new RunCommand(() -> Shooter.flywheelAmp(true), Shooter)); //new JoystickButton(OPERATOR, 1).whileTrue(ShootSpeakerCMD); - new JoystickButton(OPERATOR, 1).whileTrue(ShootSpeakerPoselessCMD); + new JoystickButton(OPERATOR, 1).whileTrue(TimedSpeakerShot); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/ShootSpeakerPoselessCommand.java b/src/main/java/frc/robot/commands/TimedSpeakerShotCommand.java similarity index 62% rename from src/main/java/frc/robot/commands/ShootSpeakerPoselessCommand.java rename to src/main/java/frc/robot/commands/TimedSpeakerShotCommand.java index 8026bcf..93160eb 100644 --- a/src/main/java/frc/robot/commands/ShootSpeakerPoselessCommand.java +++ b/src/main/java/frc/robot/commands/TimedSpeakerShotCommand.java @@ -1,36 +1,21 @@ package frc.robot.commands; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.Robot; -import frc.robot.subsystems.PoseEstimationSubsystem; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.SwerveSubsystem; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonUtils; import java.util.Map; import static edu.wpi.first.wpilibj.Timer.getFPGATimestamp; -import static frc.robot.Constants.Vision.*; -public class ShootSpeakerPoselessCommand extends Command { +public class TimedSpeakerShotCommand extends Command { private final ShooterSubsystem Shooter; private double initialTimestamp; private final GenericEntry secondsToShootEntry = Shuffleboard.getTab("config").add("seconds to shoot p", (double) 1).withWidget(BuiltInWidgets.kDial).withProperties(Map.of("min", 0, "max", 1)).getEntry(); - public ShootSpeakerPoselessCommand( + public TimedSpeakerShotCommand( ShooterSubsystem Shooter ) { this.Shooter = Shooter; diff --git a/src/main/java/frc/robot/subsystems/AlternateLEDSubsystem.java b/src/main/java/frc/robot/subsystems/AlternateLEDSubsystem.java index f7f7446..32a70dd 100644 --- a/src/main/java/frc/robot/subsystems/AlternateLEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AlternateLEDSubsystem.java @@ -14,6 +14,7 @@ public class AlternateLEDSubsystem { public presetSettings Presets; private final SendableChooser ledChooser = new SendableChooser<>(); private final PowerDistribution pd; + private final SendableChooser patternBypassChooser = new SendableChooser<>(); public AlternateLEDSubsystem() { blinkin = new AddressableLED(0); @@ -34,6 +35,12 @@ public AlternateLEDSubsystem() { ledChooser.setDefaultOption("LED on", true); Shuffleboard.getTab("config").add("LED status", ledChooser).withWidget(BuiltInWidgets.kSplitButtonChooser).withSize(2, 1); ledChooser.onChange(pd::setSwitchableChannel); + + for (BlinkinPattern pat : BlinkinPattern.values()) { + patternBypassChooser.addOption(pat.name(), pat); + } + Shuffleboard.getTab("config").add("LED bypass", patternBypassChooser); + patternBypassChooser.onChange(this::setPattern); } // https://www.chiefdelphi.com/t/rev-blinkin-example-code/452871/4