From 751b17ab248d94c6da2b9e07963d09f2adc07f08 Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Sun, 20 Jul 2025 15:54:23 -0700 Subject: [PATCH 1/7] created the statemachine I made the "shell" of the statemachine based of the given list for the states, and our drive states from on season. --- .../frc/robot/subsystems/StateMachine.java | 361 +++++++++++++++++- 1 file changed, 352 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index 22030148..9b16cbe7 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -21,12 +21,22 @@ public class StateMachine extends SubsystemBase { @NotLogged Drivetrain subDrivetrain; @NotLogged + Elevator subElevator; + @NotLogged + Intake subIntake; + @NotLogged + Climber subClimber; + @NotLogged StateMachine subStateMachine = this; /** Creates a new StateMachine. */ - public StateMachine(Drivetrain subDrivetrain) { + public StateMachine(Drivetrain subDrivetrain, Elevator subElevator, Intake subIntake, + Climber subClimber) { currentRobotState = RobotState.NONE; currentDriverState = DriverState.MANUAL; + this.subElevator = subElevator; + this.subIntake = subIntake; + this.subClimber = subClimber; this.subDrivetrain = subDrivetrain; } @@ -54,21 +64,354 @@ public Command tryState(RobotState desiredState) { case NONE: return new None(subStateMachine); } + + break; + +// climbing states + + case PREP_CLIMB: + switch (currentRobotState) { + + return new PrepClimb(subStateMachine); + + } break; - } - return Commands - .print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: " + desiredState.toString() - + " while at " + currentRobotState.toString()); - } + + case CLIMBING: + switch (currentRobotState) { + + return new Climbing(subStateMachine); + + } + break; + +// Prep Coral only + case PREP_CORAL_ZERO: + switch (currentRobotState) { + + return new PrepCoralZero(subStateMachine); + +} +break; + +case PREP_CORAL_L1: + switch (currentRobotState) { + + return new PrepCoralLv(subStateMachine); + +} +break; + +case PREP_CORAL_L2: + switch (currentRobotState) { + + return new PrepCoralLv(subStateMachine); + +} +break; + +case PREP_CORAL_L3: + switch (currentRobotState) { + + return new PrepCoralLv(subStateMachine); + +} +break; + +case PREP_CORAL_L4: + switch (currentRobotState) { + + return new PrepCoralLv(subStateMachine); + +} +break; + +// prep Coral with Algae + +case PREP_CORAL_WITH_ALGAE_L1: + switch (currentRobotState) { + + return new PrepCoralWithAlgae(subStateMachine); + +} +break; + +case PREP_CORAL_WITH_ALGAE_L2: + switch (currentRobotState) { + + return new PrepCoralWithAlgae(subStateMachine); + +} +break; + +case PREP_CORAL_WITH_ALGAE_L3: + switch (currentRobotState) { + + return new PrepCoralWithAlgae(subStateMachine); + +} +break; + +case PREP_CORAL_WITH_ALGAE_L4: + switch (currentRobotState) { + + return new PrepCoralWithAlgae(subStateMachine); + +} +break; + +case PREP_CORAL_ZERO_WITH_ALGAE: + switch (currentRobotState) { + + return new PrepCoralWithAlgae(subStateMachine); + +} +break; + +// prep Algae only + +case PREP_ALGAE_NET: + switch (currentRobotState) { + + return new PrepNet(subStateMachine); + +} +break; + +case PREP_ALGAE_PROCESSOR: + switch (currentRobotState) { + + return new PrepProcessor(subStateMachine); + +} +break; + +case PREP_ALGAE_ZERO: + switch (currentRobotState) { + + return new PrepZero(subStateMachine); + +} +break; + +// prep Algae with Coral + +case PREP_ALGAE_NET_WITH_CORAL: + switch (currentRobotState) { + + return new PrepNetWithCoral(subStateMachine); + +} +break; + +case PREP_ALGAE_PROCESSOR_WITH_CORAL: + switch (currentRobotState) { + + return new PrepProcessorWithCoral(subStateMachine); + +} +break; + +case PREP_ALGAE_ZERO_WITH_CORAL: + switch (currentRobotState) { + + return new PrepZeroWithCoral(subStateMachine); + +} +break; + +// holding 1 game piece + + case HAS_CORAL: + switch (currentRobotState) { + + return new HasCoral(subStateMachine); + } + break; + + case HAS_ALGAE: + switch (currentRobotState) { + + return new HasAlgae(subStateMachine); + } + break; + +// holding 2 game pieces + case HAS_CORAL_AND_ALGAE: + switch (currentRobotState) { + + return new HasCoralAndAlgae(subStateMachine); + } + break; + +// manipulating 1 game piece + + + case SCORING_CORAL: + switch (currentRobotState) { + + return new ScoringCoral(subStateMachine); + } + break; + + case SCORING_ALGAE: + switch (currentRobotState) { + + return new ScoringAlgae(subStateMachine); + } + break; + + case CLEAN_HIGH: + switch (currentRobotState) { + + return new CleanHigh(subStateMachine); + } + break; + + case CLEAN_LOW: + switch (currentRobotState) { + + return new CleanLow(subStateMachine); + } + break; + + case INTAKE_CORAL_STATION: + switch (currentRobotState) { + + return new IntakeCoralStation(subStateMachine); + } + break; + + case INTAKE_ALGAE_GROUND: + switch (currentRobotState) { + + return new IntakeAlgaeGround(subStateMachine); + } + break; + +// manipulating 2 game pieces + case EJECTING: + switch (currentRobotState) { + + return new Ejecting(subStateMachine); + } + break; + + case SCORING_ALGAE_WITH_CORAL: + switch (currentRobotState) { + + return new ScoringAlgaeWithCoral(subStateMachine); + } + break; + + case SCORING_CORAL_WITH_ALGAE: + switch (currentRobotState) { + + return new ScoringCoralWithAlgae(subStateMachine); + } + break; + + case CLEAN_HIGH_WITH_CORAL: + switch (currentRobotState) { + + return new CleanHighWithCoral(subStateMachine); + } + break; + + case CLEAN_LOW_WITH_CORAL: + switch (currentRobotState) { + + return new CleanLowWithCoral(subStateMachine); + } + break; + + case INTAKE_CORAL_GROUND: + switch (currentRobotState) { + + return new IntakeCoralGround(subStateMachine); + } + break; + + case INTAKE_CORAL_WITH_ALGAE_GROUND: + switch (currentRobotState) { + + return new IntakeCoralWithAlgaeGround(subStateMachine); + } + break; + + case INTAKE_ALGAE_WITH_CORAL_GROUND: + switch (currentRobotState) { + + return new IntakeAlgaeWithCoralGround(subStateMachine); + } + break; + + }return Commands.print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: "+desiredState.toString()+" while at "+currentRobotState.toString()); +} public enum DriverState { - MANUAL + MANUAL, + REEF_ROTATION_SNAPPING, + CORAL_STATION_ROTATION_SNAPPING, + REEF_AUTO_DRIVING, + CORAL_STATION_AUTO_DRIVING, + PROCESSOR_ROTATION_SNAPPING, + PROCESSOR_AUTO_DRIVING, + NET_ROTATION_SNAPPING, + NET_AUTO_DRIVING, + ALGAE_ROTATION_SNAPPING, + ALGAE_AUTO_DRIVING, + CAGE_ROTATION_SNAPPING // TODO: Add other driver states as needed } public enum RobotState { - NONE - // TODO: Add other robot states as needed + NONE, + // climbing states + PREP_CLIMB, + CLIMBING, + // Prep Coral only + PREP_CORAL_ZERO, + PREP_CORAL_L1, + PREP_CORAL_L2, + PREP_CORAL_L3, + PREP_CORAL_L4, + // prep Coral with Algae + PREP_CORAL_WITH_ALGAE_L1, + PREP_CORAL_WITH_ALGAE_L2, + PREP_CORAL_WITH_ALGAE_L3, + PREP_CORAL_WITH_ALGAE_L4, + PREP_CORAL_ZERO_WITH_ALGAE, + // prep Algae only + PREP_ALGAE_NET, + PREP_ALGAE_PROCESSOR, + PREP_ALGAE_ZERO, + // prep Algae with Coral + PREP_ALGAE_NET_WITH_CORAL, + PREP_ALGAE_PROCESSOR_WITH_CORAL, + PREP_ALGAE_ZERO_WITH_CORAL, + // holding 1 game piece + HAS_CORAL, + HAS_ALGAE, + // holding 2 game pieces + HAS_CORAL_AND_ALGAE, + // manipulating 1 game piece + SCORING_CORAL, + SCORING_ALGAE, + CLEAN_HIGH, + CLEAN_LOW, + INTAKE_CORAL_STATION, + INTAKE_ALGAE_GROUND, + // manipulating 2 game pieces + EJECTING, // we are planning on ejecting both game pieces at the same time + SCORING_ALGAE_WITH_CORAL, + SCORING_CORAL_WITH_ALGAE, + CLEAN_HIGH_WITH_CORAL, + CLEAN_LOW_WITH_CORAL, + INTAKE_CORAL_GROUND, + INTAKE_CORAL_WITH_ALGAE_GROUND, + INTAKE_ALGAE_WITH_CORAL_GROUND, + } @Override From 512f4a2d2f6350f9ce915acb04f0e277086c1f8b Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Sun, 20 Jul 2025 17:19:30 -0700 Subject: [PATCH 2/7] created the outline for all commands --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../frc/robot/commands/States/CleanHigh.java | 40 +++ .../commands/States/CleanHighWithCoral.java | 41 +++ .../frc/robot/commands/States/CleanLow.java | 40 +++ .../commands/States/CleanLowWithCoral.java | 41 +++ .../frc/robot/commands/States/Climbing.java | 40 +++ .../frc/robot/commands/States/Ejecting.java | 40 +++ .../frc/robot/commands/States/HasAlgae.java | 40 +++ .../frc/robot/commands/States/HasCoral.java | 40 +++ .../commands/States/HasCoralAndAlgae.java | 40 +++ .../commands/States/IntakeAlgaeGround.java | 41 +++ .../States/IntakeAlgaeWithCoralGround.java | 39 +++ .../commands/States/IntakeCoralGround.java | 40 +++ .../commands/States/IntakeCoralStation.java | 40 +++ .../States/IntakeCoralWithAlgaeGround.java | 41 +++ .../java/frc/robot/commands/States/None.java | 2 +- .../robot/commands/States/PrepAlgaeZero.java | 41 +++ .../States/PrepAlgaeZeroWithCoral.java | 40 +++ .../frc/robot/commands/States/PrepClimb.java | 41 +++ .../robot/commands/States/PrepCoralLv.java | 41 +++ .../commands/States/PrepCoralWithAlgae.java | 41 +++ .../robot/commands/States/PrepCoralZero.java | 41 +++ .../States/PrepCoralZeroWithAlgae.java | 41 +++ .../frc/robot/commands/States/PrepNet.java | 41 +++ .../commands/States/PrepNetWithCoral.java | 40 +++ .../robot/commands/States/PrepProcessor.java | 41 +++ .../States/PrepProcessorWithCoral.java | 40 +++ .../robot/commands/States/ScoringAlgae.java | 40 +++ .../States/ScoringAlgaeWithCoral.java | 40 +++ .../robot/commands/States/ScoringCoral.java | 40 +++ .../States/ScoringCoralWithAlgae.java | 40 +++ .../frc/robot/subsystems/StateMachine.java | 286 +++++++++--------- 32 files changed, 1327 insertions(+), 137 deletions(-) create mode 100644 src/main/java/frc/robot/commands/States/CleanHigh.java create mode 100644 src/main/java/frc/robot/commands/States/CleanHighWithCoral.java create mode 100644 src/main/java/frc/robot/commands/States/CleanLow.java create mode 100644 src/main/java/frc/robot/commands/States/CleanLowWithCoral.java create mode 100644 src/main/java/frc/robot/commands/States/Climbing.java create mode 100644 src/main/java/frc/robot/commands/States/Ejecting.java create mode 100644 src/main/java/frc/robot/commands/States/HasAlgae.java create mode 100644 src/main/java/frc/robot/commands/States/HasCoral.java create mode 100644 src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java create mode 100644 src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java create mode 100644 src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java create mode 100644 src/main/java/frc/robot/commands/States/IntakeCoralGround.java create mode 100644 src/main/java/frc/robot/commands/States/IntakeCoralStation.java create mode 100644 src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java create mode 100644 src/main/java/frc/robot/commands/States/PrepAlgaeZero.java create mode 100644 src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java create mode 100644 src/main/java/frc/robot/commands/States/PrepClimb.java create mode 100644 src/main/java/frc/robot/commands/States/PrepCoralLv.java create mode 100644 src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java create mode 100644 src/main/java/frc/robot/commands/States/PrepCoralZero.java create mode 100644 src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java create mode 100644 src/main/java/frc/robot/commands/States/PrepNet.java create mode 100644 src/main/java/frc/robot/commands/States/PrepNetWithCoral.java create mode 100644 src/main/java/frc/robot/commands/States/PrepProcessor.java create mode 100644 src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java create mode 100644 src/main/java/frc/robot/commands/States/ScoringAlgae.java create mode 100644 src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java create mode 100644 src/main/java/frc/robot/commands/States/ScoringCoral.java create mode 100644 src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec54df58..2cd667da 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,7 +25,10 @@ public class RobotContainer { private final SN_XboxController conDriver = new SN_XboxController(mapControllers.DRIVER_USB); private final Drivetrain subDrivetrain = new Drivetrain(); - private final StateMachine subStateMachine = new StateMachine(subDrivetrain); + private final Intake subIntake = new Intake(); + private final Climber subClimber = new Climber(); + private final Elevator subElevator = new Elevator(); + private final StateMachine subStateMachine = new StateMachine(subDrivetrain, subIntake, subClimber, subElevator); private final RobotPoses robotPose = new RobotPoses(subDrivetrain); Command TRY_NONE = Commands.deferredProxy( diff --git a/src/main/java/frc/robot/commands/States/CleanHigh.java b/src/main/java/frc/robot/commands/States/CleanHigh.java new file mode 100644 index 00000000..b232ae26 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/CleanHigh.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 CleanHigh extends Command { + StateMachine globalStateMachine; + + public CleanHigh(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/CleanHighWithCoral.java b/src/main/java/frc/robot/commands/States/CleanHighWithCoral.java new file mode 100644 index 00000000..e15b6996 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/CleanHighWithCoral.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 CleanHighWithCoral extends Command { + /** Creates a new CleanHighWithCoral. */ + StateMachine globalStateMachine; + + public CleanHighWithCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/CleanLow.java b/src/main/java/frc/robot/commands/States/CleanLow.java new file mode 100644 index 00000000..cc069982 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/CleanLow.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 CleanLow extends Command { + StateMachine globalStateMachine; + + public CleanLow(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/CleanLowWithCoral.java b/src/main/java/frc/robot/commands/States/CleanLowWithCoral.java new file mode 100644 index 00000000..73608a5e --- /dev/null +++ b/src/main/java/frc/robot/commands/States/CleanLowWithCoral.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 CleanLowWithCoral extends Command { + /** Creates a new CleanLowWithCoral. */ + StateMachine globalStateMachine; + + public CleanLowWithCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/Climbing.java b/src/main/java/frc/robot/commands/States/Climbing.java new file mode 100644 index 00000000..7838bf05 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/Climbing.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 Climbing extends Command { + StateMachine globalStateMachine; + + public Climbing(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/Ejecting.java b/src/main/java/frc/robot/commands/States/Ejecting.java new file mode 100644 index 00000000..97415a48 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/Ejecting.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 Ejecting extends Command { + StateMachine globalStateMachine; + + public Ejecting(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/HasAlgae.java b/src/main/java/frc/robot/commands/States/HasAlgae.java new file mode 100644 index 00000000..330a7369 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/HasAlgae.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 HasAlgae extends Command { + StateMachine globalStateMachine; + + public HasAlgae(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/HasCoral.java b/src/main/java/frc/robot/commands/States/HasCoral.java new file mode 100644 index 00000000..27518040 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/HasCoral.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 HasCoral extends Command { + StateMachine globalStateMachine; + + public HasCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java b/src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java new file mode 100644 index 00000000..71e990fe --- /dev/null +++ b/src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 HasCoralAndAlgae extends Command { + StateMachine globalStateMachine; + + public HasCoralAndAlgae(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java b/src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java new file mode 100644 index 00000000..3c2edb97 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 IntakeAlgaeGround extends Command { + /** Creates a new IntakeAlgaeGround. */ + StateMachine globalStateMachine; + + public IntakeAlgaeGround(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java b/src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java new file mode 100644 index 00000000..3119cf54 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java @@ -0,0 +1,39 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 IntakeAlgaeWithCoralGround extends Command { + StateMachine globalStateMachine; + + public IntakeAlgaeWithCoralGround(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + @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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/IntakeCoralGround.java b/src/main/java/frc/robot/commands/States/IntakeCoralGround.java new file mode 100644 index 00000000..c007357b --- /dev/null +++ b/src/main/java/frc/robot/commands/States/IntakeCoralGround.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 IntakeCoralGround extends Command { + StateMachine globalStateMachine; + + public IntakeCoralGround(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/IntakeCoralStation.java b/src/main/java/frc/robot/commands/States/IntakeCoralStation.java new file mode 100644 index 00000000..d7cfe357 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/IntakeCoralStation.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 IntakeCoralStation extends Command { + StateMachine globalStateMachine; + + public IntakeCoralStation(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java b/src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java new file mode 100644 index 00000000..25b2888f --- /dev/null +++ b/src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 IntakeCoralWithAlgaeGround extends Command { + /** Creates a new IntakeCoralWithAlgaeGround. */ + StateMachine globalStateMachine; + + public IntakeCoralWithAlgaeGround(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/None.java b/src/main/java/frc/robot/commands/States/None.java index 6864bfc3..9d6252f8 100644 --- a/src/main/java/frc/robot/commands/States/None.java +++ b/src/main/java/frc/robot/commands/States/None.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.states; +package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.*; diff --git a/src/main/java/frc/robot/commands/States/PrepAlgaeZero.java b/src/main/java/frc/robot/commands/States/PrepAlgaeZero.java new file mode 100644 index 00000000..b720fd54 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepAlgaeZero.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepAlgaeZero extends Command { + /** Creates a new PrepAlgaeZero. */ + StateMachine globalStateMachine; + + public PrepAlgaeZero(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java b/src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java new file mode 100644 index 00000000..e936cce0 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepAlgaeZeroWithCoral extends Command { + StateMachine globalStateMachine; + + public PrepAlgaeZeroWithCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepClimb.java b/src/main/java/frc/robot/commands/States/PrepClimb.java new file mode 100644 index 00000000..cef97764 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepClimb.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepClimb extends Command { + /** Creates a new PrepClimb. */ + StateMachine globalStateMachine; + + public PrepClimb(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepCoralLv.java b/src/main/java/frc/robot/commands/States/PrepCoralLv.java new file mode 100644 index 00000000..0360f6d3 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepCoralLv.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepCoralLv extends Command { + /** Creates a new PrepCoralLv. */ + StateMachine globalStateMachine; + + public PrepCoralLv(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java new file mode 100644 index 00000000..f306e591 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepCoralWithAlgae extends Command { + /** Creates a new PrepCoralWithAlgae. */ + StateMachine globalStateMachine; + + public PrepCoralWithAlgae(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepCoralZero.java b/src/main/java/frc/robot/commands/States/PrepCoralZero.java new file mode 100644 index 00000000..6c29d647 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepCoralZero.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepCoralZero extends Command { + /** Creates a new PrepCoralZero. */ + StateMachine globalStateMachine; + + public PrepCoralZero(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java b/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java new file mode 100644 index 00000000..e9b5698a --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepCoralZeroWithAlgae extends Command { + /** Creates a new PrepCoralZeroWithAlgae. */ + StateMachine globalStateMachine; + + public PrepCoralZeroWithAlgae(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepNet.java b/src/main/java/frc/robot/commands/States/PrepNet.java new file mode 100644 index 00000000..a6eed3bf --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepNet.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepNet extends Command { + /** Creates a new PrepNet. */ + StateMachine globalStateMachine; + + public PrepNet(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepNetWithCoral.java b/src/main/java/frc/robot/commands/States/PrepNetWithCoral.java new file mode 100644 index 00000000..35a773fe --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepNetWithCoral.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepNetWithCoral extends Command { + StateMachine globalStateMachine; + + public PrepNetWithCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepProcessor.java b/src/main/java/frc/robot/commands/States/PrepProcessor.java new file mode 100644 index 00000000..47081b51 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepProcessor.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepProcessor extends Command { + /** Creates a new PrepProcessor. */ + StateMachine globalStateMachine; + + public PrepProcessor(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java b/src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java new file mode 100644 index 00000000..a54d761f --- /dev/null +++ b/src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 PrepProcessorWithCoral extends Command { + StateMachine globalStateMachine; + + public PrepProcessorWithCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/ScoringAlgae.java b/src/main/java/frc/robot/commands/States/ScoringAlgae.java new file mode 100644 index 00000000..70b5a556 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/ScoringAlgae.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 ScoringAlgae extends Command { + StateMachine globalStateMachine; + + public ScoringAlgae(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java b/src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java new file mode 100644 index 00000000..8deaba80 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 ScoringAlgaeWithCoral extends Command { + StateMachine globalStateMachine; + + public ScoringAlgaeWithCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/ScoringCoral.java b/src/main/java/frc/robot/commands/States/ScoringCoral.java new file mode 100644 index 00000000..aa5d4da6 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/ScoringCoral.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 ScoringCoral extends Command { + StateMachine globalStateMachine; + + public ScoringCoral(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java new file mode 100644 index 00000000..6d99d6f1 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java @@ -0,0 +1,40 @@ +// 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 frc.robot.commands.States; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.StateMachine; + +/* 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 ScoringCoralWithAlgae extends Command { + StateMachine globalStateMachine; + + public ScoringCoralWithAlgae(StateMachine globalStateMachine) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index 9b16cbe7..c7236822 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -11,7 +11,22 @@ import edu.wpi.first.wpilibj2.command.Command.*; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.states.*; +import frc.robot.commands.States.Climbing; +import frc.robot.commands.States.HasAlgae; +import frc.robot.commands.States.HasCoral; +import frc.robot.commands.States.None; +import frc.robot.commands.States.PrepAlgaeZero; +import frc.robot.commands.States.PrepAlgaeZeroWithCoral; +import frc.robot.commands.States.PrepClimb; +import frc.robot.commands.States.PrepCoralLv; +import frc.robot.commands.States.PrepCoralWithAlgae; +import frc.robot.commands.States.PrepCoralZero; +import frc.robot.commands.States.PrepCoralZeroWithAlgae; +import frc.robot.commands.States.PrepNet; +import frc.robot.commands.States.PrepNetWithCoral; +import frc.robot.commands.States.PrepProcessor; +import frc.robot.commands.States.PrepProcessorWithCoral; +import frc.robot.commands.States.*; import frc.robot.subsystems.*; @Logged @@ -30,13 +45,13 @@ public class StateMachine extends SubsystemBase { StateMachine subStateMachine = this; /** Creates a new StateMachine. */ - public StateMachine(Drivetrain subDrivetrain, Elevator subElevator, Intake subIntake, - Climber subClimber) { + public StateMachine(Drivetrain subDrivetrain, Intake subIntake, Climber subClimber, + Elevator subElevator) { currentRobotState = RobotState.NONE; currentDriverState = DriverState.MANUAL; - this.subElevator = subElevator; this.subIntake = subIntake; this.subClimber = subClimber; + this.subElevator = subElevator; this.subDrivetrain = subDrivetrain; } @@ -64,290 +79,291 @@ public Command tryState(RobotState desiredState) { case NONE: return new None(subStateMachine); } - + break; -// climbing states + // climbing states case PREP_CLIMB: switch (currentRobotState) { - + case NONE: return new PrepClimb(subStateMachine); - + } break; case CLIMBING: switch (currentRobotState) { - + case NONE: return new Climbing(subStateMachine); - + } break; -// Prep Coral only - case PREP_CORAL_ZERO: + // Prep Coral only + case PREP_CORAL_ZERO: switch (currentRobotState) { - - return new PrepCoralZero(subStateMachine); - -} -break; + case NONE: + return new PrepCoralZero(subStateMachine); -case PREP_CORAL_L1: + } + break; + + case PREP_CORAL_L1: switch (currentRobotState) { - - return new PrepCoralLv(subStateMachine); - -} -break; + case NONE: + return new PrepCoralLv(subStateMachine); -case PREP_CORAL_L2: + } + break; + + case PREP_CORAL_L2: switch (currentRobotState) { - - return new PrepCoralLv(subStateMachine); - -} -break; + case NONE: + return new PrepCoralLv(subStateMachine); -case PREP_CORAL_L3: + } + break; + + case PREP_CORAL_L3: switch (currentRobotState) { - - return new PrepCoralLv(subStateMachine); - -} -break; + case NONE: + return new PrepCoralLv(subStateMachine); + + } + break; -case PREP_CORAL_L4: + case PREP_CORAL_L4: switch (currentRobotState) { - - return new PrepCoralLv(subStateMachine); - -} -break; + case NONE: + return new PrepCoralLv(subStateMachine); -// prep Coral with Algae - -case PREP_CORAL_WITH_ALGAE_L1: + } + break; + + // prep Coral with Algae + + case PREP_CORAL_WITH_ALGAE_L1: switch (currentRobotState) { - - return new PrepCoralWithAlgae(subStateMachine); - -} -break; + case NONE: + return new PrepCoralWithAlgae(subStateMachine); + + } + break; -case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L2: switch (currentRobotState) { - - return new PrepCoralWithAlgae(subStateMachine); - -} -break; + case NONE: + return new PrepCoralWithAlgae(subStateMachine); -case PREP_CORAL_WITH_ALGAE_L3: + } + break; + + case PREP_CORAL_WITH_ALGAE_L3: switch (currentRobotState) { - - return new PrepCoralWithAlgae(subStateMachine); - -} -break; + case NONE: + return new PrepCoralWithAlgae(subStateMachine); + + } + break; -case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_WITH_ALGAE_L4: switch (currentRobotState) { - - return new PrepCoralWithAlgae(subStateMachine); - -} -break; + case NONE: + return new PrepCoralWithAlgae(subStateMachine); + + } + break; -case PREP_CORAL_ZERO_WITH_ALGAE: + case PREP_CORAL_ZERO_WITH_ALGAE: switch (currentRobotState) { - - return new PrepCoralWithAlgae(subStateMachine); - -} -break; + case NONE: + return new PrepCoralZeroWithAlgae(subStateMachine); + + } + break; -// prep Algae only + // prep Algae only -case PREP_ALGAE_NET: + case PREP_ALGAE_NET: switch (currentRobotState) { - - return new PrepNet(subStateMachine); - -} -break; + case NONE: + return new PrepNet(subStateMachine); + + } + break; -case PREP_ALGAE_PROCESSOR: + case PREP_ALGAE_PROCESSOR: switch (currentRobotState) { - - return new PrepProcessor(subStateMachine); - -} -break; + case NONE: + return new PrepProcessor(subStateMachine); + + } + break; -case PREP_ALGAE_ZERO: + case PREP_ALGAE_ZERO: switch (currentRobotState) { - - return new PrepZero(subStateMachine); - -} -break; + case NONE: + return new PrepAlgaeZero(subStateMachine); -// prep Algae with Coral + } + break; -case PREP_ALGAE_NET_WITH_CORAL: + // prep Algae with Coral + + case PREP_ALGAE_NET_WITH_CORAL: switch (currentRobotState) { - - return new PrepNetWithCoral(subStateMachine); - -} -break; + case NONE: + return new PrepNetWithCoral(subStateMachine); -case PREP_ALGAE_PROCESSOR_WITH_CORAL: + } + break; + + case PREP_ALGAE_PROCESSOR_WITH_CORAL: switch (currentRobotState) { - - return new PrepProcessorWithCoral(subStateMachine); - -} -break; + case NONE: + return new PrepProcessorWithCoral(subStateMachine); + + } + break; -case PREP_ALGAE_ZERO_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: switch (currentRobotState) { - - return new PrepZeroWithCoral(subStateMachine); - -} -break; + case NONE: + return new PrepAlgaeZeroWithCoral(subStateMachine); + + } + break; -// holding 1 game piece + // holding 1 game piece case HAS_CORAL: switch (currentRobotState) { - + case NONE: return new HasCoral(subStateMachine); } break; case HAS_ALGAE: switch (currentRobotState) { - + case NONE: return new HasAlgae(subStateMachine); } break; -// holding 2 game pieces + // holding 2 game pieces case HAS_CORAL_AND_ALGAE: switch (currentRobotState) { - + case NONE: return new HasCoralAndAlgae(subStateMachine); } break; -// manipulating 1 game piece - + // manipulating 1 game piece case SCORING_CORAL: switch (currentRobotState) { - + case NONE: return new ScoringCoral(subStateMachine); } break; case SCORING_ALGAE: switch (currentRobotState) { - + case NONE: return new ScoringAlgae(subStateMachine); } break; case CLEAN_HIGH: switch (currentRobotState) { - + case NONE: return new CleanHigh(subStateMachine); } break; case CLEAN_LOW: switch (currentRobotState) { - + case NONE: return new CleanLow(subStateMachine); } break; case INTAKE_CORAL_STATION: switch (currentRobotState) { - + case NONE: return new IntakeCoralStation(subStateMachine); } break; case INTAKE_ALGAE_GROUND: switch (currentRobotState) { - + case NONE: return new IntakeAlgaeGround(subStateMachine); } break; -// manipulating 2 game pieces + // manipulating 2 game pieces case EJECTING: switch (currentRobotState) { - + case NONE: return new Ejecting(subStateMachine); } break; case SCORING_ALGAE_WITH_CORAL: switch (currentRobotState) { - + case NONE: return new ScoringAlgaeWithCoral(subStateMachine); } break; case SCORING_CORAL_WITH_ALGAE: switch (currentRobotState) { - + case NONE: return new ScoringCoralWithAlgae(subStateMachine); } break; case CLEAN_HIGH_WITH_CORAL: switch (currentRobotState) { - + case NONE: return new CleanHighWithCoral(subStateMachine); } break; case CLEAN_LOW_WITH_CORAL: switch (currentRobotState) { - + case NONE: return new CleanLowWithCoral(subStateMachine); } break; case INTAKE_CORAL_GROUND: switch (currentRobotState) { - + case NONE: return new IntakeCoralGround(subStateMachine); } break; case INTAKE_CORAL_WITH_ALGAE_GROUND: switch (currentRobotState) { - + case NONE: return new IntakeCoralWithAlgaeGround(subStateMachine); } break; case INTAKE_ALGAE_WITH_CORAL_GROUND: switch (currentRobotState) { - + case NONE: return new IntakeAlgaeWithCoralGround(subStateMachine); } break; - }return Commands.print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: "+desiredState.toString()+" while at "+currentRobotState.toString()); -} + } + return Commands.print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: " + + desiredState.toString() + " while at " + currentRobotState.toString()); + } public enum DriverState { MANUAL, From 89a03740cacfb0369d26994ca7cde591f6868c8b Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Tue, 22 Jul 2025 15:19:43 -0700 Subject: [PATCH 3/7] removed unnecessary imports --- .../java/frc/robot/subsystems/StateMachine.java | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index c7236822..a270d812 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -11,21 +11,6 @@ import edu.wpi.first.wpilibj2.command.Command.*; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.States.Climbing; -import frc.robot.commands.States.HasAlgae; -import frc.robot.commands.States.HasCoral; -import frc.robot.commands.States.None; -import frc.robot.commands.States.PrepAlgaeZero; -import frc.robot.commands.States.PrepAlgaeZeroWithCoral; -import frc.robot.commands.States.PrepClimb; -import frc.robot.commands.States.PrepCoralLv; -import frc.robot.commands.States.PrepCoralWithAlgae; -import frc.robot.commands.States.PrepCoralZero; -import frc.robot.commands.States.PrepCoralZeroWithAlgae; -import frc.robot.commands.States.PrepNet; -import frc.robot.commands.States.PrepNetWithCoral; -import frc.robot.commands.States.PrepProcessor; -import frc.robot.commands.States.PrepProcessorWithCoral; import frc.robot.commands.States.*; import frc.robot.subsystems.*; From 95a12d686319f6b42c390f373ee891b05ef6019d Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Wed, 23 Jul 2025 14:05:07 -0700 Subject: [PATCH 4/7] put all switches for the robot states --- .../frc/robot/commands/States/Climbing.java | 3 +- .../frc/robot/commands/States/PrepClimb.java | 5 +- .../robot/commands/States/PrepCoralLv.java | 4 +- .../commands/States/PrepCoralWithAlgae.java | 4 +- .../robot/commands/States/PrepCoralZero.java | 4 +- .../States/PrepCoralZeroWithAlgae.java | 4 +- .../frc/robot/subsystems/StateMachine.java | 214 ++++++++++++++---- 7 files changed, 192 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/commands/States/Climbing.java b/src/main/java/frc/robot/commands/States/Climbing.java index 7838bf05..32353654 100644 --- a/src/main/java/frc/robot/commands/States/Climbing.java +++ b/src/main/java/frc/robot/commands/States/Climbing.java @@ -5,13 +5,14 @@ package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.StateMachine; /* 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 Climbing extends Command { StateMachine globalStateMachine; - public Climbing(StateMachine globalStateMachine) { + public Climbing(StateMachine globalStateMachine, Elevator subElevator) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; addRequirements(globalStateMachine); diff --git a/src/main/java/frc/robot/commands/States/PrepClimb.java b/src/main/java/frc/robot/commands/States/PrepClimb.java index cef97764..4c2123ba 100644 --- a/src/main/java/frc/robot/commands/States/PrepClimb.java +++ b/src/main/java/frc/robot/commands/States/PrepClimb.java @@ -5,6 +5,9 @@ package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; /* 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 */ @@ -12,7 +15,7 @@ public class PrepClimb extends Command { /** Creates a new PrepClimb. */ StateMachine globalStateMachine; - public PrepClimb(StateMachine globalStateMachine) { + public PrepClimb(StateMachine globalStateMachine, Climber subClimber, Intake subIntake, Elevator subElevator) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; addRequirements(globalStateMachine); diff --git a/src/main/java/frc/robot/commands/States/PrepCoralLv.java b/src/main/java/frc/robot/commands/States/PrepCoralLv.java index 0360f6d3..a9f34140 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralLv.java +++ b/src/main/java/frc/robot/commands/States/PrepCoralLv.java @@ -5,6 +5,8 @@ package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; /* 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 */ @@ -12,7 +14,7 @@ public class PrepCoralLv extends Command { /** Creates a new PrepCoralLv. */ StateMachine globalStateMachine; - public PrepCoralLv(StateMachine globalStateMachine) { + public PrepCoralLv(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; addRequirements(globalStateMachine); diff --git a/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java index f306e591..a53b023c 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java @@ -5,6 +5,8 @@ package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; /* 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 */ @@ -12,7 +14,7 @@ public class PrepCoralWithAlgae extends Command { /** Creates a new PrepCoralWithAlgae. */ StateMachine globalStateMachine; - public PrepCoralWithAlgae(StateMachine globalStateMachine) { + public PrepCoralWithAlgae(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; addRequirements(globalStateMachine); diff --git a/src/main/java/frc/robot/commands/States/PrepCoralZero.java b/src/main/java/frc/robot/commands/States/PrepCoralZero.java index 6c29d647..aafc5ce7 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralZero.java +++ b/src/main/java/frc/robot/commands/States/PrepCoralZero.java @@ -5,6 +5,8 @@ package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; /* 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 */ @@ -12,7 +14,7 @@ public class PrepCoralZero extends Command { /** Creates a new PrepCoralZero. */ StateMachine globalStateMachine; - public PrepCoralZero(StateMachine globalStateMachine) { + public PrepCoralZero(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; addRequirements(globalStateMachine); diff --git a/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java b/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java index e9b5698a..0b03619e 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java @@ -5,6 +5,8 @@ package frc.robot.commands.States; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; /* 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 */ @@ -12,7 +14,7 @@ public class PrepCoralZeroWithAlgae extends Command { /** Creates a new PrepCoralZeroWithAlgae. */ StateMachine globalStateMachine; - public PrepCoralZeroWithAlgae(StateMachine globalStateMachine) { + public PrepCoralZeroWithAlgae(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; addRequirements(globalStateMachine); diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index a270d812..32c87b6e 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -72,15 +72,18 @@ public Command tryState(RobotState desiredState) { case PREP_CLIMB: switch (currentRobotState) { case NONE: - return new PrepClimb(subStateMachine); + case HAS_CORAL: + case HAS_ALGAE: + case HAS_CORAL_AND_ALGAE: + return new PrepClimb(subStateMachine, subClimber, subIntake, subElevator); } break; case CLIMBING: switch (currentRobotState) { - case NONE: - return new Climbing(subStateMachine); + case PREP_CLIMB: + return new Climbing(subStateMachine, subElevator); } break; @@ -88,40 +91,60 @@ public Command tryState(RobotState desiredState) { // Prep Coral only case PREP_CORAL_ZERO: switch (currentRobotState) { - case NONE: - return new PrepCoralZero(subStateMachine); + case HAS_CORAL: + case PREP_CORAL_L1: + case PREP_CORAL_L2: + case PREP_CORAL_L3: + case PREP_CORAL_L4: + return new PrepCoralZero(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_L1: switch (currentRobotState) { - case NONE: - return new PrepCoralLv(subStateMachine); + case HAS_CORAL: + case PREP_CORAL_L2: + case PREP_CORAL_L3: + case PREP_CORAL_L4: + case PREP_CORAL_ZERO: + return new PrepCoralLv(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_L2: switch (currentRobotState) { - case NONE: - return new PrepCoralLv(subStateMachine); + case HAS_CORAL: + case PREP_CORAL_L1: + case PREP_CORAL_L3: + case PREP_CORAL_L4: + case PREP_CORAL_ZERO: + return new PrepCoralLv(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_L3: switch (currentRobotState) { - case NONE: - return new PrepCoralLv(subStateMachine); + case HAS_CORAL: + case PREP_CORAL_L1: + case PREP_CORAL_L2: + case PREP_CORAL_L4: + case PREP_CORAL_ZERO: + return new PrepCoralLv(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_L4: switch (currentRobotState) { - case NONE: - return new PrepCoralLv(subStateMachine); + case HAS_CORAL: + case PREP_CORAL_L1: + case PREP_CORAL_L2: + case PREP_CORAL_L3: + case PREP_CORAL_ZERO: + return new PrepCoralLv(subStateMachine, subElevator, subIntake); } break; @@ -130,40 +153,75 @@ public Command tryState(RobotState desiredState) { case PREP_CORAL_WITH_ALGAE_L1: switch (currentRobotState) { - case NONE: - return new PrepCoralWithAlgae(subStateMachine); + case HAS_CORAL_AND_ALGAE: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_ZERO_WITH_ALGAE: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_WITH_ALGAE_L2: switch (currentRobotState) { - case NONE: - return new PrepCoralWithAlgae(subStateMachine); + case HAS_CORAL_AND_ALGAE: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_ZERO_WITH_ALGAE: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_WITH_ALGAE_L3: switch (currentRobotState) { - case NONE: - return new PrepCoralWithAlgae(subStateMachine); + case HAS_CORAL_AND_ALGAE: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_ZERO_WITH_ALGAE: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_WITH_ALGAE_L4: switch (currentRobotState) { - case NONE: - return new PrepCoralWithAlgae(subStateMachine); + case HAS_CORAL_AND_ALGAE: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_ZERO_WITH_ALGAE: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); } break; case PREP_CORAL_ZERO_WITH_ALGAE: switch (currentRobotState) { - case NONE: - return new PrepCoralZeroWithAlgae(subStateMachine); + case HAS_CORAL_AND_ALGAE: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: + return new PrepCoralZeroWithAlgae(subStateMachine, subElevator, subIntake); } break; @@ -172,7 +230,9 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_NET: switch (currentRobotState) { - case NONE: + case HAS_ALGAE: + case PREP_ALGAE_PROCESSOR: + case PREP_ALGAE_ZERO: return new PrepNet(subStateMachine); } @@ -180,7 +240,9 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_PROCESSOR: switch (currentRobotState) { - case NONE: + case HAS_ALGAE: + case PREP_ALGAE_ZERO: + case PREP_ALGAE_NET: return new PrepProcessor(subStateMachine); } @@ -188,7 +250,9 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_ZERO: switch (currentRobotState) { - case NONE: + case HAS_ALGAE: + case PREP_ALGAE_NET: + case PREP_ALGAE_PROCESSOR: return new PrepAlgaeZero(subStateMachine); } @@ -198,7 +262,14 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_NET_WITH_CORAL: switch (currentRobotState) { - case NONE: + case HAS_CORAL_AND_ALGAE: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_ALGAE_ZERO_WITH_CORAL: + case PREP_CORAL_ZERO_WITH_ALGAE: return new PrepNetWithCoral(subStateMachine); } @@ -206,7 +277,14 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_PROCESSOR_WITH_CORAL: switch (currentRobotState) { - case NONE: + case HAS_CORAL_AND_ALGAE: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_ALGAE_ZERO_WITH_CORAL: + case PREP_CORAL_ZERO_WITH_ALGAE: return new PrepProcessorWithCoral(subStateMachine); } @@ -214,7 +292,14 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_ZERO_WITH_CORAL: switch (currentRobotState) { - case NONE: + case HAS_CORAL_AND_ALGAE: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_ZERO_WITH_ALGAE: return new PrepAlgaeZeroWithCoral(subStateMachine); } @@ -224,14 +309,22 @@ public Command tryState(RobotState desiredState) { case HAS_CORAL: switch (currentRobotState) { - case NONE: + case INTAKE_CORAL_GROUND: + case INTAKE_CORAL_STATION: + case EJECTING: + case SCORING_ALGAE_WITH_CORAL: + return new HasCoral(subStateMachine); } break; case HAS_ALGAE: switch (currentRobotState) { - case NONE: + case CLEAN_HIGH: + case CLEAN_LOW: + case INTAKE_ALGAE_GROUND: + case SCORING_CORAL_WITH_ALGAE: + case EJECTING: return new HasAlgae(subStateMachine); } break; @@ -239,7 +332,10 @@ public Command tryState(RobotState desiredState) { // holding 2 game pieces case HAS_CORAL_AND_ALGAE: switch (currentRobotState) { - case NONE: + case INTAKE_ALGAE_WITH_CORAL_GROUND: + case INTAKE_CORAL_WITH_ALGAE_GROUND: + case CLEAN_HIGH_WITH_CORAL: + case CLEAN_LOW_WITH_CORAL: return new HasCoralAndAlgae(subStateMachine); } break; @@ -248,20 +344,29 @@ public Command tryState(RobotState desiredState) { case SCORING_CORAL: switch (currentRobotState) { - case NONE: + case PREP_CORAL_L1: + case PREP_CORAL_L2: + case PREP_CORAL_L3: + case PREP_CORAL_L4: + case PREP_CORAL_ZERO: + return new ScoringCoral(subStateMachine); } break; case SCORING_ALGAE: switch (currentRobotState) { - case NONE: + case PREP_ALGAE_NET: + case PREP_ALGAE_PROCESSOR: + case PREP_ALGAE_ZERO: return new ScoringAlgae(subStateMachine); } break; case CLEAN_HIGH: switch (currentRobotState) { + case CLEAN_LOW: + case HAS_ALGAE: case NONE: return new CleanHigh(subStateMachine); } @@ -270,6 +375,8 @@ public Command tryState(RobotState desiredState) { case CLEAN_LOW: switch (currentRobotState) { case NONE: + case CLEAN_HIGH: + case HAS_ALGAE: return new CleanLow(subStateMachine); } break; @@ -277,6 +384,8 @@ public Command tryState(RobotState desiredState) { case INTAKE_CORAL_STATION: switch (currentRobotState) { case NONE: + case INTAKE_CORAL_GROUND: + case HAS_CORAL: return new IntakeCoralStation(subStateMachine); } break; @@ -284,6 +393,7 @@ public Command tryState(RobotState desiredState) { case INTAKE_ALGAE_GROUND: switch (currentRobotState) { case NONE: + case HAS_ALGAE: return new IntakeAlgaeGround(subStateMachine); } break; @@ -292,34 +402,48 @@ public Command tryState(RobotState desiredState) { case EJECTING: switch (currentRobotState) { case NONE: + case HAS_ALGAE: + case HAS_CORAL: + case HAS_CORAL_AND_ALGAE: return new Ejecting(subStateMachine); } break; case SCORING_ALGAE_WITH_CORAL: switch (currentRobotState) { - case NONE: + case PREP_ALGAE_NET_WITH_CORAL: + case PREP_ALGAE_PROCESSOR_WITH_CORAL: + case PREP_ALGAE_ZERO_WITH_CORAL: return new ScoringAlgaeWithCoral(subStateMachine); } break; case SCORING_CORAL_WITH_ALGAE: switch (currentRobotState) { - case NONE: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_ZERO_WITH_ALGAE: + return new ScoringCoralWithAlgae(subStateMachine); } break; case CLEAN_HIGH_WITH_CORAL: switch (currentRobotState) { - case NONE: + case CLEAN_LOW_WITH_CORAL: + case HAS_CORAL: + case HAS_CORAL_AND_ALGAE: return new CleanHighWithCoral(subStateMachine); } break; case CLEAN_LOW_WITH_CORAL: switch (currentRobotState) { - case NONE: + case HAS_CORAL: + case CLEAN_HIGH_WITH_CORAL: + case HAS_CORAL_AND_ALGAE: return new CleanLowWithCoral(subStateMachine); } break; @@ -327,20 +451,30 @@ public Command tryState(RobotState desiredState) { case INTAKE_CORAL_GROUND: switch (currentRobotState) { case NONE: + case INTAKE_CORAL_STATION: + case HAS_CORAL: return new IntakeCoralGround(subStateMachine); } break; case INTAKE_CORAL_WITH_ALGAE_GROUND: switch (currentRobotState) { - case NONE: + case HAS_ALGAE: + case PREP_ALGAE_NET: + case PREP_ALGAE_PROCESSOR: + case PREP_ALGAE_ZERO: return new IntakeCoralWithAlgaeGround(subStateMachine); } break; case INTAKE_ALGAE_WITH_CORAL_GROUND: switch (currentRobotState) { - case NONE: + case HAS_CORAL: + case PREP_CORAL_WITH_ALGAE_L1: + case PREP_CORAL_WITH_ALGAE_L2: + case PREP_CORAL_WITH_ALGAE_L3: + case PREP_CORAL_WITH_ALGAE_L4: + case PREP_CORAL_ZERO: return new IntakeAlgaeWithCoralGround(subStateMachine); } break; From 3a2f71c5e4f6da16d661fc643e383aa9d428351f Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Wed, 23 Jul 2025 14:38:46 -0700 Subject: [PATCH 5/7] organized the states into labeled folders --- .../States/{ => climbing}/Climbing.java | 2 +- .../States/{ => climbing}/PrepClimb.java | 3 +- .../States/climbing/RetractingClimber.java | 41 +++++++++++++++++++ .../CleanHigh.java | 2 +- .../{ => first_scoring_element}/CleanLow.java | 2 +- .../IntakeAlgaeGround.java | 2 +- .../IntakeCoralGround.java | 2 +- .../IntakeCoralStation.java | 2 +- .../{ => hold_scoring_elements}/HasAlgae.java | 2 +- .../{ => hold_scoring_elements}/HasCoral.java | 2 +- .../HasCoralAndAlgae.java | 2 +- .../{ => prep_algae}/PrepAlgaeZero.java | 2 +- .../PrepAlgaeZeroWithCoral.java | 2 +- .../States/{ => prep_algae}/PrepNet.java | 2 +- .../{ => prep_algae}/PrepNetWithCoral.java | 2 +- .../{ => prep_algae}/PrepProcessor.java | 2 +- .../PrepProcessorWithCoral.java | 2 +- .../States/{ => prep_coral}/PrepCoralLv.java | 2 +- .../{ => prep_coral}/PrepCoralWithAlgae.java | 2 +- .../{ => prep_coral}/PrepCoralZero.java | 2 +- .../PrepCoralZeroWithAlgae.java | 2 +- .../States/{ => scoring}/ScoringAlgae.java | 2 +- .../{ => scoring}/ScoringAlgaeWithCoral.java | 2 +- .../States/{ => scoring}/ScoringCoral.java | 2 +- .../{ => scoring}/ScoringCoralWithAlgae.java | 2 +- .../CleanHighWithCoral.java | 2 +- .../CleanLowWithCoral.java | 2 +- .../Ejecting.java | 2 +- .../IntakeAlgaeWithCoralGround.java | 2 +- .../IntakeCoralWithAlgaeGround.java | 2 +- .../frc/robot/subsystems/StateMachine.java | 40 ++++++++++++++++++ 31 files changed, 111 insertions(+), 29 deletions(-) rename src/main/java/frc/robot/commands/States/{ => climbing}/Climbing.java (96%) rename src/main/java/frc/robot/commands/States/{ => climbing}/PrepClimb.java (96%) create mode 100644 src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java rename src/main/java/frc/robot/commands/States/{ => first_scoring_element}/CleanHigh.java (95%) rename src/main/java/frc/robot/commands/States/{ => first_scoring_element}/CleanLow.java (95%) rename src/main/java/frc/robot/commands/States/{ => first_scoring_element}/IntakeAlgaeGround.java (95%) rename src/main/java/frc/robot/commands/States/{ => first_scoring_element}/IntakeCoralGround.java (95%) rename src/main/java/frc/robot/commands/States/{ => first_scoring_element}/IntakeCoralStation.java (95%) rename src/main/java/frc/robot/commands/States/{ => hold_scoring_elements}/HasAlgae.java (95%) rename src/main/java/frc/robot/commands/States/{ => hold_scoring_elements}/HasCoral.java (95%) rename src/main/java/frc/robot/commands/States/{ => hold_scoring_elements}/HasCoralAndAlgae.java (95%) rename src/main/java/frc/robot/commands/States/{ => prep_algae}/PrepAlgaeZero.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_algae}/PrepAlgaeZeroWithCoral.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_algae}/PrepNet.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_algae}/PrepNetWithCoral.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_algae}/PrepProcessor.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_algae}/PrepProcessorWithCoral.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_coral}/PrepCoralLv.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_coral}/PrepCoralWithAlgae.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_coral}/PrepCoralZero.java (96%) rename src/main/java/frc/robot/commands/States/{ => prep_coral}/PrepCoralZeroWithAlgae.java (96%) rename src/main/java/frc/robot/commands/States/{ => scoring}/ScoringAlgae.java (96%) rename src/main/java/frc/robot/commands/States/{ => scoring}/ScoringAlgaeWithCoral.java (96%) rename src/main/java/frc/robot/commands/States/{ => scoring}/ScoringCoral.java (96%) rename src/main/java/frc/robot/commands/States/{ => scoring}/ScoringCoralWithAlgae.java (96%) rename src/main/java/frc/robot/commands/States/{ => second_scoring_element}/CleanHighWithCoral.java (95%) rename src/main/java/frc/robot/commands/States/{ => second_scoring_element}/CleanLowWithCoral.java (95%) rename src/main/java/frc/robot/commands/States/{ => second_scoring_element}/Ejecting.java (95%) rename src/main/java/frc/robot/commands/States/{ => second_scoring_element}/IntakeAlgaeWithCoralGround.java (95%) rename src/main/java/frc/robot/commands/States/{ => second_scoring_element}/IntakeCoralWithAlgaeGround.java (95%) diff --git a/src/main/java/frc/robot/commands/States/Climbing.java b/src/main/java/frc/robot/commands/States/climbing/Climbing.java similarity index 96% rename from src/main/java/frc/robot/commands/States/Climbing.java rename to src/main/java/frc/robot/commands/States/climbing/Climbing.java index 32353654..eecf7ac9 100644 --- a/src/main/java/frc/robot/commands/States/Climbing.java +++ b/src/main/java/frc/robot/commands/States/climbing/Climbing.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.climbing; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; diff --git a/src/main/java/frc/robot/commands/States/PrepClimb.java b/src/main/java/frc/robot/commands/States/climbing/PrepClimb.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepClimb.java rename to src/main/java/frc/robot/commands/States/climbing/PrepClimb.java index 4c2123ba..a0232ee2 100644 --- a/src/main/java/frc/robot/commands/States/PrepClimb.java +++ b/src/main/java/frc/robot/commands/States/climbing/PrepClimb.java @@ -1,8 +1,9 @@ + // 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 frc.robot.commands.States; +package frc.robot.commands.States.climbing; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Climber; diff --git a/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java b/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java new file mode 100644 index 00000000..3c8f1446 --- /dev/null +++ b/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.States.climbing; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.StateMachine; + +/* 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 RetractingClimber extends Command { + StateMachine globalStateMachine; + + public RetractingClimber(StateMachine globalStateMachine, Climber subClimber) { + // Use addRequirements() here to declare subsystem dependencies. + this.globalStateMachine = globalStateMachine; + addRequirements(globalStateMachine); + } + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/commands/States/CleanHigh.java b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java similarity index 95% rename from src/main/java/frc/robot/commands/States/CleanHigh.java rename to src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java index b232ae26..c878e84e 100644 --- a/src/main/java/frc/robot/commands/States/CleanHigh.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.first_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/CleanLow.java b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java similarity index 95% rename from src/main/java/frc/robot/commands/States/CleanLow.java rename to src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java index cc069982..58fd8e5a 100644 --- a/src/main/java/frc/robot/commands/States/CleanLow.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.first_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java similarity index 95% rename from src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java rename to src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java index 3c2edb97..c8d760d8 100644 --- a/src/main/java/frc/robot/commands/States/IntakeAlgaeGround.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.first_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/IntakeCoralGround.java b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java similarity index 95% rename from src/main/java/frc/robot/commands/States/IntakeCoralGround.java rename to src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java index c007357b..7d697676 100644 --- a/src/main/java/frc/robot/commands/States/IntakeCoralGround.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.first_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/IntakeCoralStation.java b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java similarity index 95% rename from src/main/java/frc/robot/commands/States/IntakeCoralStation.java rename to src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java index d7cfe357..6eb10cc9 100644 --- a/src/main/java/frc/robot/commands/States/IntakeCoralStation.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.first_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/HasAlgae.java b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java similarity index 95% rename from src/main/java/frc/robot/commands/States/HasAlgae.java rename to src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java index 330a7369..5bf567fc 100644 --- a/src/main/java/frc/robot/commands/States/HasAlgae.java +++ b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.hold_scoring_elements; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/HasCoral.java b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java similarity index 95% rename from src/main/java/frc/robot/commands/States/HasCoral.java rename to src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java index 27518040..ff9db28b 100644 --- a/src/main/java/frc/robot/commands/States/HasCoral.java +++ b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.hold_scoring_elements; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java similarity index 95% rename from src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java rename to src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java index 71e990fe..6c50b522 100644 --- a/src/main/java/frc/robot/commands/States/HasCoralAndAlgae.java +++ b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.hold_scoring_elements; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepAlgaeZero.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepAlgaeZero.java rename to src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java index b720fd54..d7de65f0 100644 --- a/src/main/java/frc/robot/commands/States/PrepAlgaeZero.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_algae; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java rename to src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java index e936cce0..1750dc88 100644 --- a/src/main/java/frc/robot/commands/States/PrepAlgaeZeroWithCoral.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_algae; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepNet.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepNet.java rename to src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java index a6eed3bf..b023c142 100644 --- a/src/main/java/frc/robot/commands/States/PrepNet.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_algae; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepNetWithCoral.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepNetWithCoral.java rename to src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java index 35a773fe..76e5a55f 100644 --- a/src/main/java/frc/robot/commands/States/PrepNetWithCoral.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_algae; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepProcessor.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepProcessor.java rename to src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java index 47081b51..bc7751c9 100644 --- a/src/main/java/frc/robot/commands/States/PrepProcessor.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_algae; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java rename to src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java index a54d761f..0dd85a61 100644 --- a/src/main/java/frc/robot/commands/States/PrepProcessorWithCoral.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_algae; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/PrepCoralLv.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepCoralLv.java rename to src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java index a9f34140..2e33c87d 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralLv.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_coral; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; diff --git a/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java rename to src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java index a53b023c..c8c12a7a 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_coral; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; diff --git a/src/main/java/frc/robot/commands/States/PrepCoralZero.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepCoralZero.java rename to src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java index aafc5ce7..fbbbdca5 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralZero.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_coral; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; diff --git a/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java similarity index 96% rename from src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java rename to src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java index 0b03619e..1c816bee 100644 --- a/src/main/java/frc/robot/commands/States/PrepCoralZeroWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.prep_coral; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; diff --git a/src/main/java/frc/robot/commands/States/ScoringAlgae.java b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java similarity index 96% rename from src/main/java/frc/robot/commands/States/ScoringAlgae.java rename to src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java index 70b5a556..9001481d 100644 --- a/src/main/java/frc/robot/commands/States/ScoringAlgae.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.scoring; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java similarity index 96% rename from src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java rename to src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java index 8deaba80..9b15b62b 100644 --- a/src/main/java/frc/robot/commands/States/ScoringAlgaeWithCoral.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.scoring; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/ScoringCoral.java b/src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java similarity index 96% rename from src/main/java/frc/robot/commands/States/ScoringCoral.java rename to src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java index aa5d4da6..140f58d0 100644 --- a/src/main/java/frc/robot/commands/States/ScoringCoral.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.scoring; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java similarity index 96% rename from src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java rename to src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java index 6d99d6f1..36c17b14 100644 --- a/src/main/java/frc/robot/commands/States/ScoringCoralWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.scoring; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/CleanHighWithCoral.java b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java similarity index 95% rename from src/main/java/frc/robot/commands/States/CleanHighWithCoral.java rename to src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java index e15b6996..ca9637b2 100644 --- a/src/main/java/frc/robot/commands/States/CleanHighWithCoral.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.second_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/CleanLowWithCoral.java b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java similarity index 95% rename from src/main/java/frc/robot/commands/States/CleanLowWithCoral.java rename to src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java index 73608a5e..2bea0d05 100644 --- a/src/main/java/frc/robot/commands/States/CleanLowWithCoral.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.second_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/Ejecting.java b/src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java similarity index 95% rename from src/main/java/frc/robot/commands/States/Ejecting.java rename to src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java index 97415a48..cf8fa6ce 100644 --- a/src/main/java/frc/robot/commands/States/Ejecting.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.second_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java similarity index 95% rename from src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java rename to src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java index 3119cf54..ec9de418 100644 --- a/src/main/java/frc/robot/commands/States/IntakeAlgaeWithCoralGround.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.second_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java similarity index 95% rename from src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java rename to src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java index 25b2888f..d0f2dcc6 100644 --- a/src/main/java/frc/robot/commands/States/IntakeCoralWithAlgaeGround.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java @@ -2,7 +2,7 @@ // 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 frc.robot.commands.States; +package frc.robot.commands.States.second_scoring_element; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index 32c87b6e..249fe0ae 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -12,6 +12,36 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.States.*; +import frc.robot.commands.States.climbing.Climbing; +import frc.robot.commands.States.climbing.PrepClimb; +import frc.robot.commands.States.climbing.RetractingClimber; +import frc.robot.commands.States.first_scoring_element.CleanHigh; +import frc.robot.commands.States.first_scoring_element.CleanLow; +import frc.robot.commands.States.first_scoring_element.IntakeAlgaeGround; +import frc.robot.commands.States.first_scoring_element.IntakeCoralGround; +import frc.robot.commands.States.first_scoring_element.IntakeCoralStation; +import frc.robot.commands.States.hold_scoring_elements.HasAlgae; +import frc.robot.commands.States.hold_scoring_elements.HasCoral; +import frc.robot.commands.States.hold_scoring_elements.HasCoralAndAlgae; +import frc.robot.commands.States.prep_algae.PrepAlgaeZero; +import frc.robot.commands.States.prep_algae.PrepAlgaeZeroWithCoral; +import frc.robot.commands.States.prep_algae.PrepNet; +import frc.robot.commands.States.prep_algae.PrepNetWithCoral; +import frc.robot.commands.States.prep_algae.PrepProcessor; +import frc.robot.commands.States.prep_algae.PrepProcessorWithCoral; +import frc.robot.commands.States.prep_coral.PrepCoralLv; +import frc.robot.commands.States.prep_coral.PrepCoralWithAlgae; +import frc.robot.commands.States.prep_coral.PrepCoralZero; +import frc.robot.commands.States.prep_coral.PrepCoralZeroWithAlgae; +import frc.robot.commands.States.scoring.ScoringAlgae; +import frc.robot.commands.States.scoring.ScoringAlgaeWithCoral; +import frc.robot.commands.States.scoring.ScoringCoral; +import frc.robot.commands.States.scoring.ScoringCoralWithAlgae; +import frc.robot.commands.States.second_scoring_element.CleanHighWithCoral; +import frc.robot.commands.States.second_scoring_element.CleanLowWithCoral; +import frc.robot.commands.States.second_scoring_element.Ejecting; +import frc.robot.commands.States.second_scoring_element.IntakeAlgaeWithCoralGround; +import frc.robot.commands.States.second_scoring_element.IntakeCoralWithAlgaeGround; import frc.robot.subsystems.*; @Logged @@ -79,7 +109,16 @@ public Command tryState(RobotState desiredState) { } break; + case RETRACTING_CLIMBER: + switch (currentRobotState) { + case PREP_CLIMB: + case CLIMBING: + case NONE: + return new RetractingClimber(subStateMachine, subClimber); + } + + break; case CLIMBING: switch (currentRobotState) { case PREP_CLIMB: @@ -504,6 +543,7 @@ public enum RobotState { NONE, // climbing states PREP_CLIMB, + RETRACTING_CLIMBER, CLIMBING, // Prep Coral only PREP_CORAL_ZERO, From 417d62eed64fe5263be3eb8ed1bcf68aa5ccc9b1 Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Wed, 23 Jul 2025 17:12:32 -0700 Subject: [PATCH 6/7] made the commands set the specified state --- src/main/java/frc/robot/Constants.java | 9 +++++++++ .../java/frc/robot/commands/States/None.java | 3 ++- .../commands/States/climbing/Climbing.java | 2 ++ .../commands/States/climbing/PrepClimb.java | 2 ++ .../States/climbing/RetractingClimber.java | 2 ++ .../States/first_scoring_element/CleanHigh.java | 2 ++ .../States/first_scoring_element/CleanLow.java | 2 ++ .../IntakeAlgaeGround.java | 2 ++ .../IntakeCoralGround.java | 2 ++ .../IntakeCoralStation.java | 2 ++ .../States/hold_scoring_elements/HasAlgae.java | 2 ++ .../States/hold_scoring_elements/HasCoral.java | 2 ++ .../hold_scoring_elements/HasCoralAndAlgae.java | 2 ++ .../States/prep_algae/PrepAlgaeZero.java | 2 ++ .../prep_algae/PrepAlgaeZeroWithCoral.java | 2 ++ .../commands/States/prep_algae/PrepNet.java | 2 ++ .../States/prep_algae/PrepNetWithCoral.java | 2 ++ .../States/prep_algae/PrepProcessor.java | 2 ++ .../prep_algae/PrepProcessorWithCoral.java | 2 ++ .../commands/States/prep_coral/PrepCoralLv.java | 15 ++++++++++++++- .../States/prep_coral/PrepCoralWithAlgae.java | 15 ++++++++++++++- .../States/prep_coral/PrepCoralZero.java | 2 ++ .../prep_coral/PrepCoralZeroWithAlgae.java | 2 ++ .../commands/States/scoring/ScoringAlgae.java | 2 ++ .../States/scoring/ScoringAlgaeWithCoral.java | 2 ++ .../commands/States/scoring/ScoringCoral.java | 2 ++ .../States/scoring/ScoringCoralWithAlgae.java | 2 ++ .../CleanHighWithCoral.java | 2 ++ .../CleanLowWithCoral.java | 2 ++ .../States/second_scoring_element/Ejecting.java | 2 ++ .../IntakeAlgaeWithCoralGround.java | 2 ++ .../IntakeCoralWithAlgaeGround.java | 2 ++ .../java/frc/robot/subsystems/StateMachine.java | 17 +++++++++-------- 33 files changed, 104 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f0b802e7..0d619b64 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -237,6 +237,15 @@ public static boolean isRedAlliance() { public static final Pose2d WORKSHOP_STARTING_POSE = new Pose2d(5.98, 2.60, new Rotation2d(0)); } + public static class constElevator { + + public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(0); + public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(0); + public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(0); + public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(0); + + } + public static class constVision { /** *

diff --git a/src/main/java/frc/robot/commands/States/None.java b/src/main/java/frc/robot/commands/States/None.java index 9d6252f8..03168468 100644 --- a/src/main/java/frc/robot/commands/States/None.java +++ b/src/main/java/frc/robot/commands/States/None.java @@ -4,6 +4,7 @@ package frc.robot.commands.States; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.*; @@ -22,7 +23,7 @@ public None(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { - globalStateMachine.setRobotState(StateMachine.RobotState.NONE); + globalStateMachine.setRobotState(RobotState.NONE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/climbing/Climbing.java b/src/main/java/frc/robot/commands/States/climbing/Climbing.java index eecf7ac9..74e991a3 100644 --- a/src/main/java/frc/robot/commands/States/climbing/Climbing.java +++ b/src/main/java/frc/robot/commands/States/climbing/Climbing.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; /* 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 Climbing extends Command { @@ -21,6 +22,7 @@ public Climbing(StateMachine globalStateMachine, Elevator subElevator) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.CLIMBING); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/climbing/PrepClimb.java b/src/main/java/frc/robot/commands/States/climbing/PrepClimb.java index a0232ee2..c15dbbca 100644 --- a/src/main/java/frc/robot/commands/States/climbing/PrepClimb.java +++ b/src/main/java/frc/robot/commands/States/climbing/PrepClimb.java @@ -10,6 +10,7 @@ import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; /* 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 PrepClimb extends Command { @@ -25,6 +26,7 @@ public PrepClimb(StateMachine globalStateMachine, Climber subClimber, Intake sub // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_CLIMB); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java b/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java index 3c8f1446..7c77280a 100644 --- a/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java +++ b/src/main/java/frc/robot/commands/States/climbing/RetractingClimber.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Climber; import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; /* 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 RetractingClimber extends Command { @@ -21,6 +22,7 @@ public RetractingClimber(StateMachine globalStateMachine, Climber subClimber) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.RETRACTING_CLIMBER); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java index c878e84e..5e53af86 100644 --- a/src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanHigh.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; /* 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 CleanHigh extends Command { @@ -20,6 +21,7 @@ public CleanHigh(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.CLEAN_HIGH); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java index 58fd8e5a..b0a59ee2 100644 --- a/src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/CleanLow.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; +import frc.robot.subsystems.StateMachine.RobotState; /* 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 CleanLow extends Command { @@ -20,6 +21,7 @@ public CleanLow(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.CLEAN_LOW); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java index c8d760d8..2f32315c 100644 --- a/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeAlgaeGround.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.first_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public IntakeAlgaeGround(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.INTAKE_ALGAE_GROUND); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java index 7d697676..65d6c75c 100644 --- a/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralGround.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.first_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public IntakeCoralGround(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.INTAKE_CORAL_GROUND); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java index 6eb10cc9..45ccb0e8 100644 --- a/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java +++ b/src/main/java/frc/robot/commands/States/first_scoring_element/IntakeCoralStation.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.first_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public IntakeCoralStation(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.INTAKE_CORAL_STATION); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java index 5bf567fc..9f9642ba 100644 --- a/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java +++ b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasAlgae.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.hold_scoring_elements; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public HasAlgae(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.HAS_ALGAE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java index ff9db28b..e221b9ce 100644 --- a/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java +++ b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.hold_scoring_elements; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public HasCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.HAS_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java index 6c50b522..216dc05b 100644 --- a/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java +++ b/src/main/java/frc/robot/commands/States/hold_scoring_elements/HasCoralAndAlgae.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.hold_scoring_elements; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public HasCoralAndAlgae(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.HAS_CORAL_AND_ALGAE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java index d7de65f0..f04521f4 100644 --- a/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZero.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_algae; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public PrepAlgaeZero(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_ALGAE_ZERO); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java index 1750dc88..3e0d55fe 100644 --- a/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_algae; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public PrepAlgaeZeroWithCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_ALGAE_ZERO_WITH_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java index b023c142..70b61e48 100644 --- a/src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepNet.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_algae; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public PrepNet(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_ALGAE_NET); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java index 76e5a55f..bf243b95 100644 --- a/src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepNetWithCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_algae; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public PrepNetWithCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_ALGAE_NET_WITH_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java index bc7751c9..94233d57 100644 --- a/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessor.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_algae; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public PrepProcessor(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_ALGAE_PROCESSOR); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java index 0dd85a61..36cf1567 100644 --- a/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java +++ b/src/main/java/frc/robot/commands/States/prep_algae/PrepProcessorWithCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_algae; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public PrepProcessorWithCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_ALGAE_PROCESSOR_WITH_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java index 2e33c87d..0bce0d22 100644 --- a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralLv.java @@ -4,25 +4,38 @@ package frc.robot.commands.States.prep_coral; +import frc.robot.subsystems.StateMachine.RobotState; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; +import frc.robot.Constants.constElevator; /* 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 PrepCoralLv extends Command { /** Creates a new PrepCoralLv. */ StateMachine globalStateMachine; + Distance globalDistance; - public PrepCoralLv(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake) { + public PrepCoralLv(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake, Distance height) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; + this.globalDistance = height; addRequirements(globalStateMachine); } // Called when the command is initially scheduled. @Override public void initialize() { + if (globalDistance.equals(constElevator.CORAL_L1_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_L1); + else if (globalDistance.equals(constElevator.CORAL_L2_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_L2); + else if (globalDistance.equals(constElevator.CORAL_L3_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_L3); + else if (globalDistance.equals(constElevator.CORAL_L4_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_L4); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java index c8c12a7a..28bceefc 100644 --- a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralWithAlgae.java @@ -4,25 +4,38 @@ package frc.robot.commands.States.prep_coral; +import frc.robot.subsystems.StateMachine.RobotState; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.StateMachine; +import frc.robot.Constants.constElevator; /* 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 PrepCoralWithAlgae extends Command { /** Creates a new PrepCoralWithAlgae. */ StateMachine globalStateMachine; + Distance globalDistance; - public PrepCoralWithAlgae(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake) { + public PrepCoralWithAlgae(StateMachine globalStateMachine, Elevator subElevator, Intake subIntake, Distance height) { // Use addRequirements() here to declare subsystem dependencies. this.globalStateMachine = globalStateMachine; + this.globalDistance = height; addRequirements(globalStateMachine); } // Called when the command is initially scheduled. @Override public void initialize() { + if (globalDistance.equals(constElevator.CORAL_L1_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_WITH_ALGAE_L1); + else if (globalDistance.equals(constElevator.CORAL_L2_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_WITH_ALGAE_L2); + else if (globalDistance.equals(constElevator.CORAL_L3_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_WITH_ALGAE_L3); + else if (globalDistance.equals(constElevator.CORAL_L4_HEIGHT)) + globalStateMachine.setRobotState(RobotState.PREP_CORAL_WITH_ALGAE_L4); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java index fbbbdca5..9450b5d1 100644 --- a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZero.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_coral; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; @@ -23,6 +24,7 @@ public PrepCoralZero(StateMachine globalStateMachine, Elevator subElevator, Inta // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_CORAL_ZERO); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java index 1c816bee..267bc2e7 100644 --- a/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/prep_coral/PrepCoralZeroWithAlgae.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.prep_coral; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; @@ -23,6 +24,7 @@ public PrepCoralZeroWithAlgae(StateMachine globalStateMachine, Elevator subEleva // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.PREP_CORAL_ZERO_WITH_ALGAE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java index 9001481d..ba674a13 100644 --- a/src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgae.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.scoring; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public ScoringAlgae(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.SCORING_ALGAE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java index 9b15b62b..8d897863 100644 --- a/src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringAlgaeWithCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.scoring; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public ScoringAlgaeWithCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.SCORING_ALGAE_WITH_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java b/src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java index 140f58d0..15799f6e 100644 --- a/src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.scoring; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public ScoringCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.SCORING_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java b/src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java index 36c17b14..137e9762 100644 --- a/src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java +++ b/src/main/java/frc/robot/commands/States/scoring/ScoringCoralWithAlgae.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.scoring; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public ScoringCoralWithAlgae(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.SCORING_CORAL_WITH_ALGAE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java index ca9637b2..a5aca78b 100644 --- a/src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanHighWithCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.second_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public CleanHighWithCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.CLEAN_HIGH_WITH_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java index 2bea0d05..24f6bc37 100644 --- a/src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/CleanLowWithCoral.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.second_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public CleanLowWithCoral(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.CLEAN_LOW_WITH_CORAL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java b/src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java index cf8fa6ce..b9f18500 100644 --- a/src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/Ejecting.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.second_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -20,6 +21,7 @@ public Ejecting(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.EJECTING); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java index ec9de418..093fbc73 100644 --- a/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeAlgaeWithCoralGround.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.second_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -19,6 +20,7 @@ public IntakeAlgaeWithCoralGround(StateMachine globalStateMachine) { @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.INTAKE_ALGAE_WITH_CORAL_GROUND); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java index d0f2dcc6..618f8946 100644 --- a/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java +++ b/src/main/java/frc/robot/commands/States/second_scoring_element/IntakeCoralWithAlgaeGround.java @@ -4,6 +4,7 @@ package frc.robot.commands.States.second_scoring_element; +import frc.robot.subsystems.StateMachine.RobotState; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.StateMachine; @@ -21,6 +22,7 @@ public IntakeCoralWithAlgaeGround(StateMachine globalStateMachine) { // Called when the command is initially scheduled. @Override public void initialize() { + globalStateMachine.setRobotState(RobotState.INTAKE_CORAL_WITH_ALGAE_GROUND); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index 249fe0ae..cb70c7b9 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.States.*; +import frc.robot.Constants.constElevator; import frc.robot.commands.States.climbing.Climbing; import frc.robot.commands.States.climbing.PrepClimb; import frc.robot.commands.States.climbing.RetractingClimber; @@ -147,7 +148,7 @@ public Command tryState(RobotState desiredState) { case PREP_CORAL_L3: case PREP_CORAL_L4: case PREP_CORAL_ZERO: - return new PrepCoralLv(subStateMachine, subElevator, subIntake); + return new PrepCoralLv(subStateMachine, subElevator, subIntake, constElevator.CORAL_L1_HEIGHT); } break; @@ -159,7 +160,7 @@ public Command tryState(RobotState desiredState) { case PREP_CORAL_L3: case PREP_CORAL_L4: case PREP_CORAL_ZERO: - return new PrepCoralLv(subStateMachine, subElevator, subIntake); + return new PrepCoralLv(subStateMachine, subElevator, subIntake, constElevator.CORAL_L2_HEIGHT); } break; @@ -171,7 +172,7 @@ public Command tryState(RobotState desiredState) { case PREP_CORAL_L2: case PREP_CORAL_L4: case PREP_CORAL_ZERO: - return new PrepCoralLv(subStateMachine, subElevator, subIntake); + return new PrepCoralLv(subStateMachine, subElevator, subIntake, constElevator.CORAL_L3_HEIGHT); } break; @@ -183,7 +184,7 @@ public Command tryState(RobotState desiredState) { case PREP_CORAL_L2: case PREP_CORAL_L3: case PREP_CORAL_ZERO: - return new PrepCoralLv(subStateMachine, subElevator, subIntake); + return new PrepCoralLv(subStateMachine, subElevator, subIntake, constElevator.CORAL_L4_HEIGHT); } break; @@ -200,7 +201,7 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_PROCESSOR_WITH_CORAL: case PREP_ALGAE_NET_WITH_CORAL: case PREP_ALGAE_ZERO_WITH_CORAL: - return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake, constElevator.CORAL_L1_HEIGHT); } break; @@ -215,7 +216,7 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_PROCESSOR_WITH_CORAL: case PREP_ALGAE_NET_WITH_CORAL: case PREP_ALGAE_ZERO_WITH_CORAL: - return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake, constElevator.CORAL_L2_HEIGHT); } break; @@ -230,7 +231,7 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_PROCESSOR_WITH_CORAL: case PREP_ALGAE_NET_WITH_CORAL: case PREP_ALGAE_ZERO_WITH_CORAL: - return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake, constElevator.CORAL_L3_HEIGHT); } break; @@ -245,7 +246,7 @@ public Command tryState(RobotState desiredState) { case PREP_ALGAE_PROCESSOR_WITH_CORAL: case PREP_ALGAE_NET_WITH_CORAL: case PREP_ALGAE_ZERO_WITH_CORAL: - return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake); + return new PrepCoralWithAlgae(subStateMachine, subElevator, subIntake, constElevator.CORAL_L4_HEIGHT); } break; From 988e3a441dc194df21570e84131c2874935af22f Mon Sep 17 00:00:00 2001 From: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Date: Wed, 23 Jul 2025 18:45:49 -0700 Subject: [PATCH 7/7] cleaned up the imports --- .../frc/robot/subsystems/StateMachine.java | 37 ++++--------------- 1 file changed, 7 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index cb70c7b9..8e9137d1 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -13,36 +13,13 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.States.*; import frc.robot.Constants.constElevator; -import frc.robot.commands.States.climbing.Climbing; -import frc.robot.commands.States.climbing.PrepClimb; -import frc.robot.commands.States.climbing.RetractingClimber; -import frc.robot.commands.States.first_scoring_element.CleanHigh; -import frc.robot.commands.States.first_scoring_element.CleanLow; -import frc.robot.commands.States.first_scoring_element.IntakeAlgaeGround; -import frc.robot.commands.States.first_scoring_element.IntakeCoralGround; -import frc.robot.commands.States.first_scoring_element.IntakeCoralStation; -import frc.robot.commands.States.hold_scoring_elements.HasAlgae; -import frc.robot.commands.States.hold_scoring_elements.HasCoral; -import frc.robot.commands.States.hold_scoring_elements.HasCoralAndAlgae; -import frc.robot.commands.States.prep_algae.PrepAlgaeZero; -import frc.robot.commands.States.prep_algae.PrepAlgaeZeroWithCoral; -import frc.robot.commands.States.prep_algae.PrepNet; -import frc.robot.commands.States.prep_algae.PrepNetWithCoral; -import frc.robot.commands.States.prep_algae.PrepProcessor; -import frc.robot.commands.States.prep_algae.PrepProcessorWithCoral; -import frc.robot.commands.States.prep_coral.PrepCoralLv; -import frc.robot.commands.States.prep_coral.PrepCoralWithAlgae; -import frc.robot.commands.States.prep_coral.PrepCoralZero; -import frc.robot.commands.States.prep_coral.PrepCoralZeroWithAlgae; -import frc.robot.commands.States.scoring.ScoringAlgae; -import frc.robot.commands.States.scoring.ScoringAlgaeWithCoral; -import frc.robot.commands.States.scoring.ScoringCoral; -import frc.robot.commands.States.scoring.ScoringCoralWithAlgae; -import frc.robot.commands.States.second_scoring_element.CleanHighWithCoral; -import frc.robot.commands.States.second_scoring_element.CleanLowWithCoral; -import frc.robot.commands.States.second_scoring_element.Ejecting; -import frc.robot.commands.States.second_scoring_element.IntakeAlgaeWithCoralGround; -import frc.robot.commands.States.second_scoring_element.IntakeCoralWithAlgaeGround; +import frc.robot.commands.States.climbing.*; +import frc.robot.commands.States.first_scoring_element.*; +import frc.robot.commands.States.hold_scoring_elements.*; +import frc.robot.commands.States.prep_algae.*; +import frc.robot.commands.States.prep_coral.*; +import frc.robot.commands.States.scoring.*; +import frc.robot.commands.States.second_scoring_element.*; import frc.robot.subsystems.*; @Logged