From eb5dce08df02547c53b89b2209e0c814afc22943 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 26 Feb 2025 15:40:29 -0700 Subject: [PATCH] Add if command, and speed saving --- .../java/frc4388/robot/RobotContainer.java | 85 +++++++++++++------ .../frc4388/robot/commands/IfCommand.java | 33 +++++++ .../frc4388/robot/subsystems/Elevator.java | 8 ++ .../java/frc4388/robot/subsystems/Lidar.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +++ src/main/java/frc4388/utility/SlowPeriod.java | 6 ++ 6 files changed, 117 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/IfCommand.java create mode 100644 src/main/java/frc4388/utility/SlowPeriod.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38291cc..5ba5777 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation; import java.io.File; import java.util.ArrayList; import java.util.List; +import java.util.function.BooleanSupplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; @@ -41,6 +42,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoLastApril; +import frc4388.robot.commands.IfCommand; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; @@ -117,11 +119,15 @@ public class RobotContainer { // true, false); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); private Command AprilLidarAlignL4Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT) + )), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -136,17 +142,22 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL4Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT) + )), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), new waitEndefectorRefrence(m_robotElevator), - new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT), @@ -161,17 +172,21 @@ public class RobotContainer { new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL3Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT), - // new InstantCommand(() -> System.out.println("Soup")), - // new WaitCommand(1), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT) + )), + new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -179,13 +194,19 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL3Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT) + )), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), new waitEndefectorRefrence(m_robotElevator), @@ -201,11 +222,12 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL2Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -214,12 +236,13 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL2Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -228,8 +251,8 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) - + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command placeCoral = new SequentialCommandGroup( @@ -251,7 +274,7 @@ public class RobotContainer { ); private Command leftAlgaeRemove = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -260,11 +283,12 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command rightAlgaeRemove = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -273,7 +297,8 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -449,7 +474,11 @@ public class RobotContainer { // Cancel button new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + .onTrue(new InstantCommand(() -> { + m_robotElevator.elevatorStop(); + m_robotElevator.endefectorStop(); + m_robotSwerveDrive.endSlowPeriod(); + }, m_robotElevator)); // Score prep // Prime 4 diff --git a/src/main/java/frc4388/robot/commands/IfCommand.java b/src/main/java/frc4388/robot/commands/IfCommand.java new file mode 100644 index 0000000..0f5cd47 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/IfCommand.java @@ -0,0 +1,33 @@ +package frc4388.robot.commands; + +import java.time.Instant; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// Command that is called if something is true +public class IfCommand extends InstantCommand { + BooleanSupplier isTrue; + Command trueCommand; + Command falseCommand; + + public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){ + this.isTrue = isTrue; + this.trueCommand = trueCommand; + this.falseCommand = falseCommand; + } + + public IfCommand(BooleanSupplier isTrue, Command trueCommand){ + this(isTrue, trueCommand, Commands.none()); + } + + @Override + public void execute() { + if(isTrue.getAsBoolean()) + trueCommand.schedule(); + else + falseCommand.schedule(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 933a444..5d04769 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -243,6 +243,14 @@ public class Elevator extends SubsystemBase { // } } + public boolean isL4Primed(){ + return currentState == CoordinationState.PrimedFour; + } + + public boolean isL3Primed(){ + return currentState == CoordinationState.PrimedThree; + } + public void armShuffle(){ if(!basinBeamBreak.get()){ //shuffle the coral with the arm until coral hits beam break diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 130efc3..5fad561 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -72,7 +72,7 @@ public class Lidar extends Subsystem { if(distance == -1){ s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); }else{ - s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED"); + s.addReport(ReportLevel.INFO, "LIDAR CONNECTED"); } return s; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1221930..1259be0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -390,6 +390,18 @@ public class SwerveDrive extends Subsystem { rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; + + public void startSlowPeriod() { + tmp_gear_index = gear_index; + setToSlow(); + } + + public void endSlowPeriod() { + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); + gear_index = tmp_gear_index; + } + @Override public String getSubsystemName() { return "Swerve Drive Controller"; diff --git a/src/main/java/frc4388/utility/SlowPeriod.java b/src/main/java/frc4388/utility/SlowPeriod.java new file mode 100644 index 0000000..e872708 --- /dev/null +++ b/src/main/java/frc4388/utility/SlowPeriod.java @@ -0,0 +1,6 @@ +package frc4388.utility; + +// A very stupid class to hold the state of the swerve speed setting for a short period of time +public class SlowPeriod { + +}