Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Auto align Consistency improvements

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-28 11:46:48 -07:00
parent a063f4b309
commit 90684ffc7b
7 changed files with 85 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+34 -51
View File
@@ -28,7 +28,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.AutoConstants;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -39,11 +39,10 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
// 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;
@@ -123,11 +122,11 @@ public class RobotContainer {
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), 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, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
@@ -135,7 +134,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -143,7 +142,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -153,9 +152,11 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
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, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -163,7 +164,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -172,7 +173,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
@@ -182,15 +183,16 @@ public class RobotContainer {
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// 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 GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
), () -> m_robotElevator.isL3Primed()),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
@@ -204,18 +206,19 @@ public class RobotContainer {
private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
// 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 GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
),() -> m_robotElevator.isL3Primed()),
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),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -231,14 +234,14 @@ public class RobotContainer {
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -246,46 +249,28 @@ public class RobotContainer {
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command placeCoral = new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
new InstantCommand(() -> System.out.println("Placing Some Coral")),
new WaitCommand(3),
new InstantCommand(() -> System.out.println("Done placing Some Coral"))
);
private Command aprilAlign = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Aligning...")),
new WaitCommand(1)
);
private Command grabCoral = new SequentialCommandGroup(
new InstantCommand(() -> System.out.println("Yoinking some coral...")),
new WaitCommand(2),
new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
);
private Command leftAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -295,11 +280,11 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -319,12 +304,10 @@ public class RobotContainer {
NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar)
));
NamedCommands.registerCommand("grab-coral", grabCoral);
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> { // Called on first robot enable