diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 882b2dc..6e73085 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -374,6 +374,9 @@ public final class Constants { public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3); + + public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); + // Test april tag field layout diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0ae6321..ba3194d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -116,7 +116,7 @@ public class RobotContainer { // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // true, false); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); - private Command AprilLidarAlign = new SequentialCommandGroup( + private Command AprilLidarAlignL4Right = new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), // new InstantCommand(() -> System.out.println("Soup")), @@ -132,7 +132,7 @@ public class RobotContainer { new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) ); - private Command AprilLidarAlignLeft = new SequentialCommandGroup( + private Command AprilLidarAlignL4Left = 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(() -> System.out.println("Soup")), @@ -165,7 +165,7 @@ public class RobotContainer { new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) ); - private Command AprilLidarAlignL3 = new SequentialCommandGroup( + private Command AprilLidarAlignL3Right = new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), @@ -185,19 +185,31 @@ public class RobotContainer { // new Translation2d(0,1), new Translation2d(), 500, true), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) ); + + private Command AprilLidarAlignL2Left = new SequentialCommandGroup( + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.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(), 500, true), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) - private Command AprilLidarLeft = new SequentialCommandGroup( - AprilLidarAlign.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_lidar) ); - private Command AprilLidarRight = new SequentialCommandGroup( - AprilLidarAlign.asProxy(), - new InstantCommand(() -> System.out.println("Soup")), - new WaitCommand(1), - new LidarAlign(m_robotSwerveDrive, m_lidar)//, - // new MoveForTimeCommand(m_robotSwerveDrive, - // new Translation2d(0, 0.5), new Translation2d(), 1000, true) + private Command AprilLidarAlignL2Right = new SequentialCommandGroup( + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.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(), 500, true), + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + ); private Command placeCoral = new SequentialCommandGroup( @@ -217,14 +229,40 @@ public class RobotContainer { new WaitCommand(2), new InstantCommand(() -> System.out.println("Done yoinking some coral...")) ); + + private Command leftAlgaeRemove = new SequentialCommandGroup( + 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 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 InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + ); + + private Command rightAlgaeRemove = new SequentialCommandGroup( + 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 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 InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + ); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlign); - NamedCommands.registerCommand("april-allign", AprilLidarAlign); + NamedCommands.registerCommand("AutoGotoPosition", AprilLidarAlignL4Right); + NamedCommands.registerCommand("april-allign", AprilLidarAlignL4Right); NamedCommands.registerCommand("place-coral", placeCoral); NamedCommands.registerCommand("grab-coral", grabCoral); @@ -318,6 +356,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) @@ -333,30 +373,13 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown())); - - - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(AprilLidarAlignLeft); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(AprilLidarAlign); + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(AprilLidarAlign); - - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(AprilLidarAlign); - // ? /* Operator Buttons */ - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue(AutoGotoPosition); - .onTrue(AprilLidarRight); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); @@ -368,12 +391,36 @@ public class RobotContainer { DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + + new JoystickButton(getButtonBox(), ButtonBox.Five) + .onTrue(AprilLidarAlignL4Left); + + new JoystickButton(getButtonBox(), ButtonBox.One) + .onTrue(AprilLidarAlignL4Right); + + new JoystickButton(getButtonBox(), ButtonBox.Six) .onTrue(AprilLidarAlignL3Left); - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(AprilLidarAlignL3); - + new JoystickButton(getButtonBox(), ButtonBox.Two) + .onTrue(AprilLidarAlignL3Right); + + new JoystickButton(getButtonBox(), ButtonBox.Seven) + .onTrue(AprilLidarAlignL2Left); + + new JoystickButton(getButtonBox(), ButtonBox.Three) + .onTrue(AprilLidarAlignL2Right); + + + // Lower coral removal + new JoystickButton(getButtonBox(), ButtonBox.Eight) + .onTrue(leftAlgaeRemove); + + // Upper coral removal + new JoystickButton(getButtonBox(), ButtonBox.Four) + .onTrue(rightAlgaeRemove); + + new JoystickButton(getButtonBox(), ButtonBox.White) + .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); //Trims new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) @@ -389,33 +436,6 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); - - // Lower coral removal - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) - .onTrue(new SequentialCommandGroup( - 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 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) - )); - - // Upper coral removal - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - .onTrue(new SequentialCommandGroup( - 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 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) - )); - // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 72137f4..5a3fe33 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -178,7 +178,7 @@ public class Elevator extends SubsystemBase { public boolean elevatorAtRefrence() { // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); - boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.1; + boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5; if (atPos) { SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence); SmartDashboard.putNumber("Elevator Pos", elevatorPosition);