From a063f4b309792cee0a5fbb20af53e143ba19f0bf Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 27 Feb 2025 19:45:18 -0700 Subject: [PATCH 1/5] Fix limit switch and maybe autos --- .../pathplanner/autos/Line-up-no-arm.auto | 2 +- src/main/java/frc4388/robot/Constants.java | 7 +++ .../java/frc4388/robot/RobotContainer.java | 44 +++++++++---------- .../frc4388/robot/commands/LidarAlign.java | 7 ++- .../frc4388/robot/subsystems/Elevator.java | 9 +++- 5 files changed, 42 insertions(+), 27 deletions(-) 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) { From 90684ffc7b4316656bf2ef8fa0612cb07c9c5edb Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 28 Feb 2025 11:46:48 -0700 Subject: [PATCH 2/5] Auto align Consistency improvements --- src/main/java/frc4388/robot/Constants.java | 89 +++++++++---------- .../java/frc4388/robot/RobotContainer.java | 85 +++++++----------- .../frc4388/robot/commands/GotoLastApril.java | 2 +- .../frc4388/robot/commands/IfCommand.java | 33 ------- .../frc4388/robot/subsystems/Elevator.java | 2 +- .../java/frc4388/robot/subsystems/Lidar.java | 8 +- .../frc4388/utility/ReefPositionHelper.java | 5 +- 7 files changed, 85 insertions(+), 139 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/IfCommand.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5781bae..c7b9799 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -229,29 +229,7 @@ public final class Constants { public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST } - - public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0.01,0.0); - public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); - public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); - public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); - public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); - public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); - - public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole - public static final int LIDAR_DIO_CHANNEL = 7; - public static final int LIDAR_MICROS_TO_CM = 10; - public static final int SECONDS_TO_MICROS = 1000000; - - public static final double XY_TOLERANCE = 0.07; // Meters - public static final double ROT_TOLERANCE = 5; // Degrees - - // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); - // public static final Pose2d targetpos = - } - - public static final class Configurations { public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. @@ -342,6 +320,48 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } + public static final class LiDARConstants { + public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole + public static final int LIDAR_DIO_CHANNEL = 7; + public static final int LIDAR_MICROS_TO_CM = 10; + public static final int SECONDS_TO_MICROS = 1000000; + } + + public static final class AutoConstants { + public static final Gains XY_GAINS = new Gains(3,0.01,0.0); + public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); + public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); + public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); + + public static final double XY_TOLERANCE = 0.07; // Meters + public static final double ROT_TOLERANCE = 5; // Degrees + + // X is tangent to reef side + // Y is normal to reef side + public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field + public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); + + public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + + public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + + public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); + + + public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(3); + + 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; + } + public static final class VisionConstants { public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; @@ -360,31 +380,6 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5); - - public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); - - // 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(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); - public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); - - 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); - - 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 // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a2b38e9..5dcbe54 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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 diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b9fd1be..5f7f285 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; import frc4388.utility.Gains; diff --git a/src/main/java/frc4388/robot/commands/IfCommand.java b/src/main/java/frc4388/robot/commands/IfCommand.java deleted file mode 100644 index 0f5cd47..0000000 --- a/src/main/java/frc4388/robot/commands/IfCommand.java +++ /dev/null @@ -1,33 +0,0 @@ -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 7397386..388dc91 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ElevatorConstants; import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.AutoConstants; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; import frc4388.utility.TimesNegativeOne; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 5fad561..b903e17 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.LiDARConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; @@ -16,7 +16,7 @@ import frc4388.utility.Status.ReportLevel; public class Lidar extends Subsystem { private double distance = -1; - Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL); + Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL); public Lidar() { LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured @@ -29,7 +29,7 @@ public class Lidar extends Subsystem { if(LidarPWM.get() < 1) distance = -1; else - distance = (LidarPWM.getPeriod() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM; + distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; } public double getDistance(){ @@ -38,7 +38,7 @@ public class Lidar extends Subsystem { public boolean withinDistance(){ if(distance == -1) return false; - return distance < AutoConstants.LIDAR_DETECT_DISTANCE; + return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; } ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index e8cb0f2..a3ab63e 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc4388.robot.Constants.AutoConstants; import frc4388.robot.Constants.FieldConstants; public class ReefPositionHelper { @@ -79,9 +80,9 @@ public class ReefPositionHelper { public static double getSide(Side side){ switch(side) { case LEFT: - return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET); + return -(AutoConstants.X_SCORING_POSITION_OFFSET); case RIGHT: - return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET); + return (AutoConstants.X_SCORING_POSITION_OFFSET); case CENTER: return 0; } From dc8a80fc461e94516398116b62f98d4e9e4d9e49 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 28 Feb 2025 11:55:14 -0700 Subject: [PATCH 3/5] Forgot to change a value --- src/main/java/frc4388/robot/Constants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c7b9799..15c07a8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -352,8 +352,7 @@ public final class Constants { public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1); - - public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(3); + public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final int L4_DRIVE_TIME = 250; //Milliseconds // public static final int L3_DRIVE_TIME = 500; From 870dfa2e6785e0f1be5e175e2eb59fe11e66a1f6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 28 Feb 2025 14:21:32 -0700 Subject: [PATCH 4/5] Make everything use our Subsystem class, improve LED controller --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 7 ++- .../java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/subsystems/DiffDrive.java | 4 -- .../frc4388/robot/subsystems/Elevator.java | 24 +++++++++- .../java/frc4388/robot/subsystems/LED.java | 46 +++++++++++++++++-- .../java/frc4388/robot/subsystems/Lidar.java | 8 +++- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../java/frc4388/robot/subsystems/Vision.java | 12 +++++ 9 files changed, 90 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 15c07a8..28f2924 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -357,7 +357,6 @@ public final class Constants { 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; } @@ -372,7 +371,8 @@ public final class Constants { // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate - // (Fake values. Experiment and determine estimation noise on an actual robot.) + // X, Y, Theta + // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2377656..092de15 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,7 +10,6 @@ package frc4388.robot; import java.util.ArrayList; import java.util.Base64; import java.util.List; -import java.util.logging.Level; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; @@ -87,9 +86,9 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - SmartDashboard.putNumber("Time", System.currentTimeMillis()); - //System.out.println(m_robotContainer.limelight.isNearSpeaker()); - //mled.updateLED(); + // SmartDashboard.putNumber("Time", System.currentTimeMillis()); + + m_robotContainer.m_robotLED.update(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5dcbe54..39cb625 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -84,7 +84,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final LED m_robotLED = new LED(); + public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED); diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index 0cffe0b..a95c0a7 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -7,21 +7,17 @@ package frc4388.robot.subsystems; -import java.util.logging.Level; - import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; import frc4388.utility.Status; import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; /** * Add your docs here. diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 388dc91..0389d90 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -23,9 +23,12 @@ import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.AutoConstants; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; import frc4388.utility.TimesNegativeOne; +import frc4388.utility.Status.ReportLevel; -public class Elevator extends SubsystemBase { +public class Elevator extends Subsystem { /** Creates a new Elevator. */ private TalonFX elevatorMotor; private TalonFX endeffectorMotor; @@ -303,4 +306,23 @@ public class Elevator extends SubsystemBase { //shuffle the coral with the arm until coral hits beam break } } + + @Override + public String getSubsystemName() { + return "Elevator"; + } + + @Override + public void queryStatus() {} + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name()); + status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor); + status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor); + + return status; + } } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 41fa708..e85100f 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -9,29 +9,67 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; import frc4388.utility.LEDPatterns; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; /** * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED * Driver */ -public class LED extends SubsystemBase { +public class LED extends Subsystem { private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - private double mode = LEDConstants.DEFAULT_PATTERN.getValue(); + private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; public void setMode(LEDPatterns pattern){ - this.mode = pattern.getValue(); + this.mode = pattern; } @Override public void periodic() { - LEDController.set(mode); + update(); } + public void update() { + if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return; + + if(DriverStation.isDisabled()){ + LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue()); + }else + LEDController.set(mode.getValue()); + } + + @Override + public String getSubsystemName() { + return "LEDs"; + } + + @Override + public void queryStatus() { + SmartDashboard.putString("LED status", mode.name()); + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + if(LEDController.isAlive()) + status.addReport(ReportLevel.INFO, "LED is CONNECTED"); + else + status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED"); + + status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name()); + + return status; + } + + } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index b903e17..3ac5cff 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -69,11 +69,15 @@ public class Lidar extends Subsystem { @Override public Status diagnosticStatus() { Status s = new Status(); + if(distance == -1){ - s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); + s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); }else{ - s.addReport(ReportLevel.INFO, "LIDAR CONNECTED"); + s.addReport(ReportLevel.INFO, "LiDAR Connected"); } + + s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance); + return s; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index eff4e3c..a790bb6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -423,7 +423,7 @@ public class SwerveDrive extends Subsystem { public Status diagnosticStatus() { Status status = new Status(); - status.addReport(ReportLevel.ERROR, + status.addReport(ReportLevel.INFO, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); return status; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index a66a00a..ed5ebe9 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -36,6 +36,7 @@ import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; public class Vision extends Subsystem { @@ -311,6 +312,17 @@ public class Vision extends Subsystem { public Status diagnosticStatus() { Status status = new Status(); + if(cameras[0].isConnected()) + status.addReport(ReportLevel.INFO, "Left Camera Connected"); + else + status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED"); + + if(cameras[1].isConnected()) + status.addReport(ReportLevel.INFO, "Right Camera Connected"); + else + status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED"); + + return status; } From bd67c9e8d346411f26ec1134176f3956e5dd047f Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 28 Feb 2025 18:46:30 -0700 Subject: [PATCH 5/5] a small change, autos work. --- .../pathplanner/autos/Blue Top Cage.auto | 16 +----- .../pathplanner/autos/Red Bottom Cage.auto | 16 +----- .../pathplanner/autos/Red Center Cage.auto | 22 +++----- .../deploy/pathplanner/autos/Stupid auto.auto | 31 ----------- .../deploy/pathplanner/autos/Test Auto.auto | 19 ------- .../paths/Bottom Blue Cage to Reef.path | 4 +- .../paths/Bottom Feed to Reef.path | 16 +++--- .../paths/Bottom Reef to Feed.path | 12 ++--- .../paths/Center Barge to Reef.path | 12 ++--- .../paths/Center Blue Cage to Reef.path | 4 +- .../paths/Center Red Cage to Reef.path | 6 +-- .../paths/Center Reef to Bottom Feed.path | 12 ++--- .../paths/Center Reef to Feed.path | 24 ++++----- .../pathplanner/paths/Stupid auto 1.path | 54 ------------------- .../pathplanner/paths/Stupid auto 2.path | 54 ------------------- .../pathplanner/paths/Stupid auto 3.path | 54 ------------------- src/main/deploy/pathplanner/paths/Taxi.path | 4 +- .../paths/Top Blue Cage to Reef.path | 8 +-- .../pathplanner/paths/Top Feed to Reef.path | 20 +++---- .../paths/Top Red Cage to Reef.path | 12 ++--- .../pathplanner/paths/Top Reef to Feed.path | 16 +++--- .../java/frc4388/robot/RobotContainer.java | 15 ++++-- .../frc4388/robot/commands/waitFeedCoral.java | 36 +++++++++++++ .../frc4388/robot/subsystems/Elevator.java | 4 ++ 24 files changed, 139 insertions(+), 332 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Stupid auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/Test Auto.auto delete mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 1.path delete mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 2.path delete mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 3.path create mode 100644 src/main/java/frc4388/robot/commands/waitFeedCoral.java diff --git a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto index 3acc852..555bb6c 100644 --- a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto +++ b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto @@ -13,13 +13,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } }, { @@ -43,13 +37,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto index 658c0b4..3439f6b 100644 --- a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto +++ b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto @@ -13,13 +13,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } }, { @@ -43,13 +37,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Red Center Cage.auto b/src/main/deploy/pathplanner/autos/Red Center Cage.auto index 47338f7..7bfeb66 100644 --- a/src/main/deploy/pathplanner/autos/Red Center Cage.auto +++ b/src/main/deploy/pathplanner/autos/Red Center Cage.auto @@ -13,13 +13,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-right-l4" } }, { @@ -28,6 +22,12 @@ "pathName": "Bottom Reef to Feed" } }, + { + "type": "named", + "data": { + "name": "align-feed" + } + }, { "type": "named", "data": { @@ -43,13 +43,7 @@ { "type": "named", "data": { - "name": "april-allign" - } - }, - { - "type": "named", - "data": { - "name": "place-coral" + "name": "place-coral-left-l4" } } ] diff --git a/src/main/deploy/pathplanner/autos/Stupid auto.auto b/src/main/deploy/pathplanner/autos/Stupid auto.auto deleted file mode 100644 index 06f26cf..0000000 --- a/src/main/deploy/pathplanner/autos/Stupid auto.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Stupid auto 1" - } - }, - { - "type": "path", - "data": { - "pathName": "Stupid auto 2" - } - }, - { - "type": "path", - "data": { - "pathName": "Stupid auto 3" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto deleted file mode 100644 index 2e09e4c..0000000 --- a/src/main/deploy/pathplanner/autos/Test Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Stupid auto 1" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path index 0670521..f63510a 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.35013649242443 + "rotation": -121.26879518614867 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.34296858150984 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path index cadc03f..25f3281 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.115768324814807, - "y": 1.5107373181346522 + "x": 4.39747159090909, + "y": 1.3027556818181814 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.692929292929293, - "y": 2.6833333333333327 + "x": 5.3048863636363635, + "y": 2.5292613636363637 }, "prevControl": { - "x": 3.11332763609877, - "y": 2.3131602797293294 + "x": 5.893210227272728, + "y": 1.9808238636363626 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -121.27770286686648 + "rotation": 114.22774531795413 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -126.57303097851931 + "rotation": 53.446462811652175 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path index 0e53fb5..6bb67cc 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.1565656565656575, - "y": 2.886616161616162 + "x": 5.344772727272727, + "y": 2.6688636363636364 }, "prevControl": null, "nextControl": { - "x": 5.532638888888889, - "y": 2.530871212121211 + "x": 6.4715625, + "y": 1.432386363636364 }, "isLocked": false, "linkedName": null @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -123.50343698241421 + "rotation": 55.05578949900953 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.036243467926624 + "rotation": 121.42956561483854 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path index ebcc6cf..cfbbb95 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path @@ -8,19 +8,19 @@ }, "prevControl": null, "nextControl": { - "x": 8.114457611671098, - "y": 4.043616195025507 + "x": 7.269289772727273, + "y": 4.064886363636362 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.979861111111112, + "x": 6.172414772727272, "y": 4.045328282828282 }, "prevControl": { - "x": 5.5035379766577375, + "x": 5.696091638273898, "y": 4.0462802990619195 }, "nextControl": null, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.5456575934157175 + "rotation": -179.96096735022735 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -179.2890804030095 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path index 2e99234..434e054 100644 --- a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.851928154286895 + "rotation": -120.86136963407526 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 1.1457628387483283 + "rotation": -179.99261564513898 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path index a2d7feb..7e6f511 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 5.96969696969697, - "y": 1.128219696969696 + "y": 1.1282196969696958 }, "isLocked": false, "linkedName": null @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -60.52411099675423 + "rotation": 122.87136694597014 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 1.97493401088202 + "rotation": 179.5295681977034 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path index 07e870b..200490f 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.786742424242425, - "y": 4.035164141414142 + "x": 6.092642045454545, + "y": 3.9651704545454542 }, "prevControl": null, "nextControl": { - "x": 7.026767676767678, - "y": 2.185290404040403 + "x": 9.024289772727268, + "y": 3.446647727272728 }, "isLocked": false, "linkedName": null @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -124.91183021661 + "rotation": 52.857102599919905 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -1.6365770416167795 + "rotation": -179.46454101443553 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path index 891990b..5bee8d7 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 5.817234848484849, - "y": 4.045328282828282 + "x": 5.982954545454545, + "y": 4.03497159090909 }, "prevControl": null, "nextControl": { - "x": 6.62020202020202, - "y": 4.797474747474748 + "x": 7.109744318181819, + "y": 3.8355397727272718 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2175505050505055, - "y": 5.45814393939394 + "x": 6.232244318181818, + "y": 5.281420454545455 }, "prevControl": { - "x": 5.45268897997343, - "y": 5.373232823449551 + "x": 7.1397939460232385, + "y": 4.676387369317844 }, "nextControl": { - "x": 3.388005050505051, - "y": 6.1188131313131295 + "x": 5.574119318181818, + "y": 5.720170454545454 }, "isLocked": false, "linkedName": null @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 126.3843518158359 + "rotation": -53.93780932479241 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.1574757392596 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 1.path b/src/main/deploy/pathplanner/paths/Stupid auto 1.path deleted file mode 100644 index afb7b0c..0000000 --- a/src/main/deploy/pathplanner/paths/Stupid auto 1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.081502525252525, - "y": 7.267361111111111 - }, - "prevControl": null, - "nextControl": { - "x": 4.871969696969696, - "y": 8.070328282828283 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.081502525252525, - "y": 5.10239898989899 - }, - "prevControl": { - "x": 4.821148989898989, - "y": 5.10239898989899 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 2.path b/src/main/deploy/pathplanner/paths/Stupid auto 2.path deleted file mode 100644 index a849084..0000000 --- a/src/main/deploy/pathplanner/paths/Stupid auto 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.101830808080808, - "y": 5.041414141414141 - }, - "prevControl": null, - "nextControl": { - "x": 7.10183080808081, - "y": 5.041414141414141 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.585795454545455, - "y": 6.149305555555555 - }, - "prevControl": { - "x": 8.175315656565658, - "y": 5.295517676767678 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 3.path b/src/main/deploy/pathplanner/paths/Stupid auto 3.path deleted file mode 100644 index c87eab2..0000000 --- a/src/main/deploy/pathplanner/paths/Stupid auto 3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.585795454545455, - "y": 6.200126262626262 - }, - "prevControl": null, - "nextControl": { - "x": 7.778914141414141, - "y": 7.033585858585859 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.081502525252525, - "y": 7.32834595959596 - }, - "prevControl": { - "x": 7.595959595959596, - "y": 7.724747474747475 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index 7b32a44..c4ec989 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.9251837083231407 + "rotation": 179.59913485447024 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -1.1306909512379957 + "rotation": -179.85896618052556 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path index 321e817..66ebba7 100644 --- a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.464431818181818, - "y": 5.6403977272727275 + "x": 5.344772727272727, + "y": 5.391107954545454 }, "prevControl": { - "x": 5.240820707070706, - "y": 5.254160353535354 + "x": 5.121161616161616, + "y": 5.00487058080808 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path index be71b4a..fce25be 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0807449494949497, - "y": 6.972601010101011 + "x": 1.116818181818182, + "y": 6.956647727272727 }, "prevControl": null, "nextControl": { - "x": 2.0869949494949496, - "y": 6.291603535353536 + "x": 2.123068181818182, + "y": 6.275650252525252 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.764078282828282, - "y": 5.24469696969697 + "x": 3.6196875, + "y": 5.35122159090909 }, "prevControl": { - "x": 2.9836492542425197, - "y": 5.685899280282051 + "x": 2.8392584714142375, + "y": 5.792423901494171 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 121.34521414171566 + "rotation": -59.239047023115106 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 124.23611780915067 + "rotation": -52.93323259086456 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path index cfabb8b..0cefb5a 100644 --- a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 8.242449416720314, - "y": 3.1104783377417067 + "x": 7.319147727272727, + "y": 2.3198579545454536 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6934974747474745 }, "prevControl": { - "x": 4.985594317577342, - "y": 2.6542548990156853 + "x": 6.401761363636364, + "y": 1.5819602272727276 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -54.659893078442295 + "rotation": 123.49004753500587 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -178.9832812445108 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path index e9543c9..40748ed 100644 --- a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.17689393939394, - "y": 5.224368686868687 + "x": 5.284943181818181, + "y": 5.401079545454545 }, "prevControl": null, "nextControl": { - "x": 5.9669821266547345, - "y": 4.892012503312241 + "x": 4.844554924242423, + "y": 6.744466540404039 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.003093434343434 }, "prevControl": { - "x": 0.960567140436829, - "y": 7.086188101328755 + "x": 1.9644034090909093, + "y": 5.750085227272726 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 126.86989764584399 + "rotation": -54.83470564502973 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 57.21571913413089 + "rotation": -121.0370223454415 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 39cb625..3a4b441 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; +import frc4388.robot.commands.waitFeedCoral; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -295,6 +296,13 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator)); + NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0, 1), + new Translation2d(), 200, true + ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); @@ -642,9 +650,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); @@ -659,7 +667,8 @@ public class RobotContainer { for (String auto : autos) { if (auto.endsWith(".auto")) - autoChooser.addOption(auto.replaceAll(".auto", ""), auto); + autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", "")); + // System.out.println(auto); } autoChooser.onChange((filename) -> { diff --git a/src/main/java/frc4388/robot/commands/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/waitFeedCoral.java new file mode 100644 index 0000000..e1e32f6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitFeedCoral.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Elevator; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitFeedCoral extends Command { + /** Creates a new waitElevatorRefrence. */ + private Elevator elevator; + public waitFeedCoral(Elevator elevator) { + this.elevator = elevator; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevator.hasCoral(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 0389d90..bd08f8b 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -301,6 +301,10 @@ public class Elevator extends Subsystem { return currentState == CoordinationState.PrimedThree; } + public boolean hasCoral(){ + return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); + } + public void armShuffle(){ if(!basinBeamBreak.get()){ //shuffle the coral with the arm until coral hits beam break