mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Merge branch 'master' into driver-preference-controls
This commit is contained in:
Michael Mikovsky
@@ -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;
|
||||
@@ -42,6 +43,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;
|
||||
@@ -64,8 +66,10 @@ import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
// Utilites
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.DeferredBlockMulti;
|
||||
import frc4388.utility.ReefPositionHelper;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.TimesNegativeOne;
|
||||
import frc4388.utility.ReefPositionHelper.Side;
|
||||
import frc4388.utility.configurable.ConfigurableString;
|
||||
|
||||
@@ -118,11 +122,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),
|
||||
@@ -137,35 +145,51 @@ 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_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
|
||||
// new InstantCommand(() -> System.out.println("Soup")),
|
||||
// new WaitCommand(1),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
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 waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(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 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),
|
||||
@@ -173,13 +197,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),
|
||||
@@ -195,11 +225,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),
|
||||
@@ -208,12 +239,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),
|
||||
@@ -222,8 +254,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(
|
||||
@@ -245,7 +277,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),
|
||||
@@ -254,11 +286,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),
|
||||
@@ -267,7 +300,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();})
|
||||
);
|
||||
|
||||
|
||||
@@ -293,7 +327,12 @@ public class RobotContainer {
|
||||
|
||||
configureButtonBindings();
|
||||
configureVirtualButtonBindings();
|
||||
new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
|
||||
new DeferredBlock(() -> { // Called on first robot enable
|
||||
m_robotSwerveDrive.resetGyro();
|
||||
});
|
||||
new DeferredBlockMulti(() -> { // Called on every robot enable
|
||||
TimesNegativeOne.update();
|
||||
});
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// CameraServer.startAutomaticCapture();
|
||||
|
||||
@@ -451,9 +490,13 @@ public class RobotContainer {
|
||||
.onTrue(rightAlgaeRemove);
|
||||
|
||||
|
||||
// Cancel button
|
||||
new JoystickButton(getButtonBox(), ButtonBox.White)
|
||||
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
|
||||
// Cancel button
|
||||
new JoystickButton(getButtonBox(), ButtonBox.White)
|
||||
.onTrue(new InstantCommand(() -> {
|
||||
m_robotElevator.elevatorStop();
|
||||
m_robotElevator.endefectorStop();
|
||||
m_robotSwerveDrive.endSlowPeriod();
|
||||
}, m_robotElevator));
|
||||
|
||||
// Score prep
|
||||
// Prime 4
|
||||
@@ -616,7 +659,7 @@ public class RobotContainer {
|
||||
// return autoCommand;
|
||||
// } catch (Exception e) {
|
||||
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
||||
return Commands.none();
|
||||
return autoCommand;
|
||||
// }
|
||||
// return new PathPlannerAuto("Line-up-no-arm");
|
||||
// zach told me to do the below comment
|
||||
@@ -638,9 +681,9 @@ public class RobotContainer {
|
||||
|
||||
autoChooser.onChange((filename) -> {
|
||||
autoCommand = new PathPlannerAuto(filename);
|
||||
System.out.println("Robot Auto Changed");
|
||||
System.out.println("Robot Auto Changed " + filename);
|
||||
});
|
||||
SmartDashboard.putData(autoChooser);
|
||||
// SmartDashboard.putData(autoChooser);
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user