From 0d70bb43110fd2f7ec4fec7e84e142ec2ed08e41 Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Mon, 24 Feb 2025 17:14:06 -0700 Subject: [PATCH 1/4] remove, incompatible old taxi --- .../frc4388/robot/commands/Autos/Taxi.txt | 225 ------------------ 1 file changed, 225 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/Autos/Taxi.txt diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt deleted file mode 100644 index c99ee2c..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt +++ /dev/null @@ -1,225 +0,0 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,12 -0.0,0.0,0.0,0.0,26 -0.0,0.0,0.0,0.0,37 -0.0,0.0,0.0,0.0,50 -0.0,0.0,0.0,0.0,62 -0.0,0.0,0.0,0.0,73 -0.0,0.0,0.0,0.0,88 -0.0,0.0,0.0,0.0,103 -0.0,0.0,0.0,0.0,116 -0.0,0.0,0.0,0.0,160 -0.0,0.0,0.0,0.0,173 -0.0,0.0,0.0,0.0,185 -0.0,0.0,0.0,0.0,200 -0.0,0.0,0.0,0.0,211 -0.0,0.0,0.0,0.0,223 -0.0,0.0,0.0,0.0,235 -0.0,0.0,0.0,0.0,247 -0.0,0.0,0.0,0.0,263 -0.0,0.0,0.0,0.0,283 -0.0,0.0,0.0,0.0,303 -0.0,-0.109375,0.0,0.0,323 -0.0,-0.1484375,0.0,0.0,343 -0.0,-0.2109375,0.0,0.0,363 -0.0,-0.3671875,0.0,0.0,398 -0.0,-0.4140625,0.0,0.0,411 -0.0,-0.4765625,0.0,0.0,425 -0.0,-0.5078125,0.0,0.0,443 -0.0,-0.5078125,0.0,0.0,463 -0.0,-0.53125,0.0,0.0,483 -0.0,-0.5546875,0.0,0.0,504 -0.0,-0.5625,0.0,0.0,523 -0.0,-0.5625,0.0,0.0,544 -0.0,-0.5703125,0.0,0.0,563 -0.0,-0.5859375,0.0,0.0,584 -0.0,-0.5859375,0.0,0.0,603 -0.0,-0.5859375,0.0,0.0,640 -0.0,-0.59375,0.0,0.0,657 -0.0,-0.6015625,0.0,0.0,672 -0.0,-0.6015625,0.0,0.0,685 -0.0,-0.6015625,0.0,0.0,703 -0.0,-0.6015625,0.0,0.0,723 -0.0,-0.6015625,0.0,0.0,743 -0.0,-0.6015625,0.0,0.0,763 -0.0,-0.6015625,0.0,0.0,783 -0.0,-0.6015625,0.0,0.0,803 -0.0,-0.6015625,0.0,0.0,823 -0.0,-0.6015625,0.0,0.0,844 -0.0,-0.6015625,0.0,0.0,878 -0.0,-0.6015625,0.0,0.0,893 -0.0,-0.6015625,0.0,0.0,907 -0.0,-0.6015625,0.0,0.0,924 -0.0,-0.609375,0.0,0.0,943 -0.0,-0.609375,0.0,0.0,963 -0.0,-0.609375,0.0,0.0,983 -0.0,-0.609375,0.0,0.0,1004 -0.0,-0.609375,0.0,0.0,1023 -0.0,-0.609375,0.0,0.0,1043 -0.0,-0.609375,0.0,0.0,1064 -0.0,-0.609375,0.0,0.0,1083 -0.0,-0.609375,0.0,0.0,1156 -0.0,-0.609375,0.0,0.0,1172 -0.0,-0.609375,0.0,0.0,1185 -0.0,-0.609375,0.0,0.0,1200 -0.0,-0.609375,0.0,0.0,1215 -0.0,-0.609375,0.0,0.0,1225 -0.0,-0.609375,0.0,0.0,1236 -0.0,-0.609375,0.0,0.0,1249 -0.0,-0.609375,0.0,0.0,1263 -0.0,-0.609375,0.0,0.0,1283 -0.0,-0.609375,0.0,0.0,1303 -0.0,-0.609375,0.0,0.0,1323 -0.0,-0.609375,0.0,0.0,1363 -0.0,-0.6015625,0.0,0.0,1376 -0.0,-0.6015625,0.0,0.0,1394 -0.0,-0.6015625,0.0,0.0,1405 -0.0,-0.6015625,0.0,0.0,1423 -0.0,-0.6015625,0.0,0.0,1443 -0.0,-0.6015625,0.0,0.0,1463 -0.0,-0.6015625,0.0,0.0,1483 -0.0,-0.6015625,0.0,0.0,1503 -0.0,-0.6015625,0.0,0.0,1523 -0.0,-0.6015625,0.0,0.0,1543 -0.0,-0.6015625,0.0,0.0,1563 -0.0,-0.6015625,0.0,0.0,1597 -0.0,-0.6015625,0.0,0.0,1608 -0.0,-0.6015625,0.0,0.0,1624 -0.0,-0.6015625,0.0,0.0,1643 -0.0,-0.6015625,0.0,0.0,1664 -0.0,-0.5859375,0.0,0.0,1683 -0.0,-0.5859375,0.0,0.0,1703 -0.0,-0.5625,0.0,0.0,1723 -0.0,-0.5625,0.0,0.0,1743 -0.0,-0.5625,0.0,0.0,1763 -0.0,-0.5625,0.0,0.0,1783 -0.0,-0.5625,0.0,0.0,1803 -0.0,-0.5625,0.0,0.0,1843 -0.0,-0.5625,0.0,0.0,1855 -0.0,-0.5625,0.0,0.0,1868 -0.0,-0.5625,0.0,0.0,1883 -0.0,-0.5625,0.0,0.0,1903 -0.0,-0.5625,0.0,0.0,1923 -0.0,-0.5625,0.0,0.0,1943 -0.0,-0.5625,0.0,0.0,1963 -0.0,-0.5625,0.0,0.0,1983 -0.0,-0.5625,0.0,0.0,2003 -0.0,-0.5625,0.0,0.0,2024 -0.0,-0.5625,0.0,0.0,2043 -0.0,-0.5625,0.0,0.0,2081 -0.0,-0.5625,0.0,0.0,2093 -0.0,-0.5625,0.0,0.0,2105 -0.0,-0.5625,0.0,0.0,2123 -0.0,-0.5625,0.0,0.0,2143 -0.0,-0.5625,0.0,0.0,2163 -0.0,-0.5625,0.0,0.0,2183 -0.0,-0.5625,0.0,0.0,2203 -0.0,-0.5625,0.0,0.0,2223 -0.0,-0.5625,0.0,0.0,2243 -0.0,-0.5625,0.0,0.0,2263 -0.0,-0.5625,0.0,0.0,2283 -0.0,-0.5625,0.0,0.0,2366 -0.0,-0.5625,0.0,0.0,2377 -0.0,-0.5625,0.0,0.0,2394 -0.0,-0.5703125,0.0,0.0,2405 -0.0,-0.5703125,0.0,0.0,2418 -0.0,-0.5703125,0.0,0.0,2431 -0.0,-0.5703125,0.0,0.0,2444 -0.0,-0.5703125,0.0,0.0,2458 -0.0,-0.5703125,0.0,0.0,2470 -0.0,-0.5703125,0.0,0.0,2485 -0.0,-0.5703125,0.0,0.0,2503 -0.0,-0.5703125,0.0,0.0,2523 -0.0,-0.5703125,0.0,0.0,2563 -0.0,-0.5703125,0.0,0.0,2577 -0.0,-0.5703125,0.0,0.0,2591 -0.0,-0.5703125,0.0,0.0,2608 -0.0,-0.5703125,0.0,0.0,2624 -0.0,-0.5703125,0.0,0.0,2643 -0.0,-0.5703125,0.0,0.0,2677 -0.0,-0.5703125,0.0,0.0,2698 -0.0,-0.5703125,0.0,0.0,2711 -0.0,-0.5703125,0.0,0.0,2725 -0.0,-0.5703125,0.0,0.0,2743 -0.0,-0.5703125,0.0,0.0,2764 -0.0,-0.5703125,0.0,0.0,2810 -0.0,-0.5703125,0.0,0.0,2820 -0.0,-0.5703125,0.0,0.0,2833 -0.0,-0.5703125,0.0,0.0,2845 -0.0,-0.5703125,0.0,0.0,2863 -0.0,-0.5703125,0.0,0.0,2883 -0.0,-0.5703125,0.0,0.0,2904 -0.0,-0.5703125,0.0,0.0,2924 -0.0,-0.5703125,0.0,0.0,2943 -0.0,-0.5703125,0.0,0.0,2963 -0.0,-0.5703125,0.0,0.0,2983 -0.0,-0.5703125,0.0,0.0,3003 -0.0,-0.5703125,0.0,0.0,3033 -0.0,-0.5703125,0.0,0.0,3050 -0.0,-0.5703125,0.0,0.0,3065 -0.0,-0.5703125,0.0,0.0,3083 -0.0,-0.5703125,0.0,0.0,3103 -0.0,-0.5703125,0.0,0.0,3123 -0.0,-0.5703125,0.0,0.0,3144 -0.0,-0.5703125,0.0,0.0,3164 -0.0,-0.5703125,0.0,0.0,3184 -0.0,-0.5703125,0.0,0.0,3203 -0.0,-0.5703125,0.0,0.0,3223 -0.0,-0.5703125,0.0,0.0,3243 -0.0,-0.5703125,0.0,0.0,3272 -0.0,-0.5703125,0.0,0.0,3289 -0.0,-0.5703125,0.0,0.0,3303 -0.0,-0.5703125,0.0,0.0,3323 -0.0,-0.5703125,0.0,0.0,3343 -0.0,-0.5703125,0.0,0.0,3363 -0.0,-0.5703125,0.0,0.0,3383 -0.0,-0.5703125,0.0,0.0,3403 -0.0,-0.5703125,0.0,0.0,3423 -0.0,-0.5703125,0.0,0.0,3443 -0.0,-0.5703125,0.0,0.0,3463 -0.0,-0.5703125,0.0,0.0,3483 -0.0,-0.5703125,0.0,0.0,3566 -0.0,-0.5703125,0.0,0.0,3578 -0.0,-0.5703125,0.0,0.0,3596 -0.0,-0.5703125,0.0,0.0,3610 -0.0,-0.5703125,0.0,0.0,3623 -0.0,-0.5703125,0.0,0.0,3640 -0.0,-0.5703125,0.0,0.0,3651 -0.0,-0.5703125,0.0,0.0,3663 -0.0,-0.5703125,0.0,0.0,3678 -0.0,-0.5703125,0.0,0.0,3691 -0.0,-0.5703125,0.0,0.0,3706 -0.0,-0.5703125,0.0,0.0,3723 -0.0,-0.5703125,0.0,0.0,3766 -0.0,-0.5703125,0.0,0.0,3778 -0.0,-0.5703125,0.0,0.0,3792 -0.0,-0.5703125,0.0,0.0,3807 -0.0,-0.5703125,0.0,0.0,3823 -0.0,-0.5703125,0.0,0.0,3843 -0.0,-0.53125,0.0,0.0,3863 -0.0,-0.53125,0.0,0.0,3884 -0.0,-0.421875,0.0,0.0,3904 -0.0,0.0,0.0,0.0,3924 -0.0,0.0,0.0,0.0,3944 -0.0,0.0,0.0,0.0,3963 -0.0,0.0,0.0,0.0,3999 -0.0,0.0,0.0,0.0,4010 -0.0,0.0,0.0,0.0,4025 -0.0,0.0,0.0,0.0,4043 -0.0,0.0,0.0,0.0,4063 -0.0,0.0,0.0,0.0,4083 -0.0,0.0,0.0,0.0,4103 -0.0,0.0,0.0,0.0,4123 -0.0,0.0,0.0,0.0,4143 -0.0,0.0,0.0,0.0,4163 -0.0,0.0,0.0,0.0,4183 -0.0,0.0,0.0,0.0,4203 -0.0,0.0,0.0,0.0,4236 -0.0,0.0,0.0,0.0,4247 -0.0,0.0,0.0,0.0,4264 -0.0,0.0,0.0,0.0,4284 -0.0,0.0,0.0,0.0,4304 -0.0,0.0,0.0,0.0,4324 -0.0,0.0,0.0,0.0,4343 -0.0,0.0,0.0,0.0,4363 \ No newline at end of file From f9eb029b668326e75f01fc0eb8991de7d7255915 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 25 Feb 2025 11:33:46 -0700 Subject: [PATCH 2/4] de stall l4 --- src/main/java/frc4388/robot/Constants.java | 4 +-- .../java/frc4388/robot/RobotContainer.java | 17 +++++++++---- .../frc4388/robot/subsystems/Elevator.java | 25 +++++++++++-------- 3 files changed, 29 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6e73085..653b130 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -419,7 +419,7 @@ public final class Constants { //Max for elevator = 50% public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; - public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe + public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe public static final double SCORING_THREE_ELEVATOR = -9.25; public static final double DEALGAE_L2_ELEVATOR = -23.5; @@ -437,7 +437,7 @@ public final class Constants { public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value - public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_TOP_ENDEFECTOR = 0.51 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() .withKP(1) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 00e4f50..38291cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -141,19 +141,26 @@ public class RobotContainer { 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 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) ); @@ -529,7 +536,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 @@ -551,9 +558,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); } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 0601c39..83aca39 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -178,20 +178,25 @@ 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.5; - if (atPos) { - SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence); - SmartDashboard.putNumber("Elevator Pos", elevatorPosition); - } + double diffrence = Math.abs(elevatorPosition) - Math.abs(elevatorRefrence); - return atPos; + boolean headedUp = diffrence < 0; + boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; + + return (Math.abs(diffrence) >= 99999 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); } - + public boolean endefectorAtRefrence() { // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); + double diffrence = Math.abs(endefectorPosition) - Math.abs(endefectorRefrence); - return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2; + boolean headedUp = diffrence < 0; + boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0; + + return (Math.abs(diffrence) >= 99999 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { @@ -210,7 +215,7 @@ public class Elevator extends SubsystemBase { // } private void periodicReady() { - if (elevatorAtRefrence()) + if (elevatorMotor.getForwardLimit().asSupplier().get().value == 0) transitionState(CoordinationState.Hovering); } @@ -227,7 +232,7 @@ public class Elevator extends SubsystemBase { SmartDashboard.putString("State", currentState.toString()); if (currentState == CoordinationState.Waiting) { - // periodicWaiting(); + periodicWaiting(); } else if (currentState == CoordinationState.WatingBeamTriped) { // periodicWaitingTripped(); } else if (currentState == CoordinationState.Ready) { From bab7469379b0cf15689471f1ffc629217c587cc0 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 25 Feb 2025 16:59:19 -0700 Subject: [PATCH 3/4] destall prt 2 --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/subsystems/Elevator.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 653b130..6cab177 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -366,7 +366,7 @@ public final class Constants { // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right - public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 83aca39..933a444 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -178,25 +178,25 @@ public class Elevator extends SubsystemBase { public boolean elevatorAtRefrence() { // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); - double diffrence = Math.abs(elevatorPosition) - Math.abs(elevatorRefrence); + double diffrence = elevatorRefrence - elevatorPosition; boolean headedUp = diffrence < 0; boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; - return (Math.abs(diffrence) >= 99999 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); } public boolean endefectorAtRefrence() { // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); - double diffrence = Math.abs(endefectorPosition) - Math.abs(endefectorRefrence); + double diffrence = endefectorRefrence - endefectorPosition; boolean headedUp = diffrence < 0; boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0; boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0; - return (Math.abs(diffrence) >= 99999 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { 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 4/4] 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 { + +}