diff --git a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto b/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto index 9fbd56c..4d51c0d 100644 --- a/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto +++ b/src/main/deploy/pathplanner/autos/Line-up-no-arm.auto @@ -13,7 +13,7 @@ { "type": "named", "data": { - "name": "lineup-no-arm" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9a5b419..5781bae 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -377,6 +377,13 @@ public final class Constants { public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); + public static final int L4_DRIVE_TIME = 250; //Milliseconds + // public static final int L3_DRIVE_TIME = 500; + public static final int L2_DRIVE_TIME = 250; //Milliseconds + + + public static final int ALGAE_DRIVE_TIME = 500; + // Test april tag field layout diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 52f5bb1..a2b38e9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -124,10 +124,10 @@ public class RobotContainer { private Command AprilLidarAlignL4Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + // 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), @@ -143,7 +143,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), 500, true), + new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) @@ -152,10 +152,10 @@ public class RobotContainer { private Command AprilLidarAlignL4Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + // 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 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), @@ -172,7 +172,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), 500, true), + new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), @@ -182,10 +182,10 @@ public class RobotContainer { private Command AprilLidarAlignL3Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new IfCommand(() -> m_robotElevator.isL3Primed(), 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, FieldConstants.L3_DISTANCE_2, Side.LEFT), + // )), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -204,10 +204,10 @@ public class RobotContainer { private Command AprilLidarAlignL3Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new IfCommand(() -> m_robotElevator.isL3Primed(), 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, 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), @@ -238,7 +238,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), 500, true), + new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) @@ -253,7 +253,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), 500, true), + new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -313,10 +313,10 @@ public class RobotContainer { NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); - NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left); - NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right); - NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left); - NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right); + NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); + NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); + NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); + 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), @@ -659,9 +659,9 @@ public class RobotContainer { // return autoCommand; // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - return autoCommand; + // return autoCommand; // } - // return new PathPlannerAuto("Line-up-no-arm"); + return new PathPlannerAuto("Line-up-no-arm"); // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 4ae9ad4..ce5c147 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -22,6 +22,7 @@ public class LidarAlign extends Command { private boolean headedRight; private double speed; private int bounces; + private double additionalDistance = 0; // private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ @@ -41,6 +42,7 @@ public class LidarAlign extends Command { this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.additionalDistance = 0; this.bounces = 0; } @@ -54,11 +56,12 @@ public class LidarAlign extends Command { return; } - if (currentFinderTick > 10) { //arbutrary threshhold for now. + if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now. headedRight = !headedRight; currentFinderTick *= -1; bounces++; - if (bounces == 2) return; + additionalDistance += 5; + if (bounces == 5) return; } double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index f885ca7..7397386 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -229,6 +229,8 @@ public class Elevator extends SubsystemBase { private void periodicWaiting() { if (!basinBeamBreak.get()) transitionState(CoordinationState.Ready); + if(!endeffectorLimitSwitch.get()) + transitionState(CoordinationState.Hovering); } // private void periodicWaitingTripped() { @@ -237,12 +239,15 @@ public class Elevator extends SubsystemBase { // } private void periodicReady() { - if (elevatorAtReference()) + if (elevatorAtReference() && !endeffectorLimitSwitch.get()) transitionState(CoordinationState.Hovering); + if(elevatorAtReference() && endeffectorLimitSwitch.get()) + transitionState(CoordinationState.Waiting); } private void periodicScoring() { - if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); + if (!endeffectorLimitSwitch.get()) + transitionState(CoordinationState.Waiting); } public void manualElevatorVel(double velocity) {