From 8e344d12a695312e865596c83426376bfb4778e6 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Thu, 5 Sep 2024 18:48:16 -0700 Subject: [PATCH 01/10] start of robot manager --- .../frc/robot/robot_manager/RobotManager.java | 62 ++++++++++++++++++- .../frc/robot/robot_manager/RobotState.java | 5 +- 2 files changed, 65 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index b985eda..277acd0 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -1,11 +1,47 @@ package frc.robot.robot_manager; +import frc.robot.arm.ArmState; +import frc.robot.arm.ArmSubsystem; +import frc.robot.imu.ImuSubsystem; +import frc.robot.intake.IntakeState; +import frc.robot.intake.IntakeSubsystem; +import frc.robot.localization.LocalizationSubsystem; +import frc.robot.queuer.QueuerState; +import frc.robot.queuer.QueuerSubsystem; +import frc.robot.shooter.ShooterState; +import frc.robot.shooter.ShooterSubsystem; import frc.robot.util.scheduling.SubsystemPriority; import frc.robot.util.state_machines.StateMachine; +import frc.robot.vision.VisionSubsystem; public class RobotManager extends StateMachine { - public RobotManager() { + public final ArmSubsystem arm; + public final ShooterSubsystem shooter; + public final LocalizationSubsystem localization; + public final VisionSubsystem vision; + private final ImuSubsystem imu; + public final IntakeSubsystem intake; + public final QueuerSubsystem queuer; + + private final double distanceToFeedSpot = 0; + private final double distanceToSpeaker = 0; + + public RobotManager( + ArmSubsystem arm, + ShooterSubsystem shooter, + LocalizationSubsystem localization, + VisionSubsystem vision, + ImuSubsystem imu, + IntakeSubsystem intake, + QueuerSubsystem queuer) { super(SubsystemPriority.ROBOT_MANAGER, RobotState.IDLE_NO_GP); + this.arm = arm; + this.shooter = shooter; + this.localization = localization; + this.vision = vision; + this.imu = imu; + this.intake = intake; + this.queuer = queuer; } @Override @@ -14,5 +50,29 @@ protected void collectInputs() {} @Override public void robotPeriodic() { super.robotPeriodic(); + // state transition + switch (getState()) { + case SPEAKER_PREPARE_TO_SCORE -> { + if (shooter.atGoal()) { + setStateFromRequest(RobotState.SPEAKER_SCORING); + } + } + } + // on state change + switch (getState()) { + case SPEAKER_PREPARE_TO_SCORE -> { + arm.setState(ArmState.SPEAKER_SHOT); + shooter.setState(ShooterState.SPEAKER_SHOT); + intake.setState(IntakeState.INTAKING); + queuer.seState(QueuerState.SHOOTING); + } + } + // continous sate action + switch (getState()) { + case SPEAKER_PREPARE_TO_SCORE -> { + shooter.setDistanceToFeedSpot(distanceToFeedSpot); + shooter.setDistanceToSpeaker(distanceToSpeaker); + } + } } } diff --git a/src/main/java/frc/robot/robot_manager/RobotState.java b/src/main/java/frc/robot/robot_manager/RobotState.java index 5c10c37..39cda4f 100644 --- a/src/main/java/frc/robot/robot_manager/RobotState.java +++ b/src/main/java/frc/robot/robot_manager/RobotState.java @@ -23,5 +23,8 @@ public enum RobotState { PASS_WAITING, PASS_PREPARE_TO_SHOOT, - PASS_SHOOTING; + PASS_SHOOTING, + + CLIMBING_1_LINEUP, + CLIMBING_2_HANGING; } From 3002d99cc0eea75f8ada3a203128bd51966397ee Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sat, 7 Sep 2024 11:50:36 -0700 Subject: [PATCH 02/10] add states to arm and shooter + continue robot manager --- src/main/java/frc/robot/arm/ArmAngle.java | 1 + src/main/java/frc/robot/arm/ArmState.java | 1 + src/main/java/frc/robot/arm/ArmSubsystem.java | 6 ++ .../frc/robot/robot_manager/RobotManager.java | 78 ++++++++++++++++--- .../java/frc/robot/shooter/ShooterRpms.java | 2 + .../java/frc/robot/shooter/ShooterState.java | 4 +- .../frc/robot/shooter/ShooterSubsystem.java | 6 ++ 7 files changed, 85 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmAngle.java b/src/main/java/frc/robot/arm/ArmAngle.java index 0cf350a..ffdfc95 100644 --- a/src/main/java/frc/robot/arm/ArmAngle.java +++ b/src/main/java/frc/robot/arm/ArmAngle.java @@ -11,4 +11,5 @@ public class ArmAngle { public static final Rotation2d CLIMBING_1_LINEUP = Rotation2d.fromDegrees(30); public static final Rotation2d CLIMBING_2_HANGING = Rotation2d.fromDegrees(20); public static final Rotation2d AMP = Rotation2d.fromDegrees(20); + public static final Rotation2d PASS = Rotation2d.fromDegrees(5); } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index fd3d8d8..c8cb36b 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -10,6 +10,7 @@ public enum ArmState { CLIMBING_1_LINEUP, CLIMBING_2_HANGING, AMP, + PASS, PRE_MATCH_HOMING; } diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 055fabe..6f0829b 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -92,6 +92,8 @@ public boolean atGoal() { case AMP -> MathUtil.isNear(ArmAngle.AMP.getDegrees(), leftMotorAngle, 1) && MathUtil.isNear(ArmAngle.AMP.getDegrees(), rightMotorAngle, 1); + case PASS -> + MathUtil.isNear(ArmAngle.PASS.getDegrees(), leftMotorAngle, 1)&&MathUtil.isNear(ArmAngle.PASS.getDegrees(), rightMotorAngle, 1); }; } @@ -155,6 +157,10 @@ protected void afterTransition(ArmState newState) { leftMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations())); rightMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations())); } + case PASS->{ + leftMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations())); + rightMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations())); + } } } diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 277acd0..189fded 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -19,7 +19,7 @@ public class RobotManager extends StateMachine { public final ShooterSubsystem shooter; public final LocalizationSubsystem localization; public final VisionSubsystem vision; - private final ImuSubsystem imu; + public final ImuSubsystem imu; public final IntakeSubsystem intake; public final QueuerSubsystem queuer; @@ -43,36 +43,90 @@ public RobotManager( this.intake = intake; this.queuer = queuer; } - + @Override protected void collectInputs() {} - @Override - public void robotPeriodic() { - super.robotPeriodic(); + protected RobotState getNexState(RobotState currentState){ // state transition - switch (getState()) { + switch (currentState) { case SPEAKER_PREPARE_TO_SCORE -> { - if (shooter.atGoal()) { + if (shooter.atGoal()&&arm.atGoal()) { setStateFromRequest(RobotState.SPEAKER_SCORING); } } + case AMP_PREPARE_TO_SCORE->{ + if (shooter.atGoal()&&arm.atGoal()){ + setStateFromRequest(RobotState.AMP_SCORING); + } + } + case FEEDING_PREPARE_TO_SHOOT ->{ + if (shooter.atGoal()&&arm.atGoal()){ + setStateFromRequest(RobotState.FEEDING_SHOOTING); + } + + } + case PASS_PREPARE_TO_SHOOT->{ + if (shooter.atGoal()&&arm.atGoal()){ + setStateFromRequest(RobotState.PASS_SHOOTING); + } + } + + } - // on state change - switch (getState()) { + return currentState; + } + @Override + protected void afterTransition(RobotState newState){ + // on state change + switch (newState) { case SPEAKER_PREPARE_TO_SCORE -> { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); - intake.setState(IntakeState.INTAKING); - queuer.seState(QueuerState.SHOOTING); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_WITH_GP); + } + case AMP_PREPARE_TO_SCORE -> { + arm.setState(ArmState.AMP); + shooter.setState(ShooterState.AMP); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_WITH_GP); + } + case FEEDING_PREPARE_TO_SHOOT -> { + arm.setState(ArmState.FEEDING); + shooter.setState(ShooterState.FEEDING); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_WITH_GP); + } + case PASS_PREPARE_TO_SHOOT -> { + arm.setState(ArmState.PASS); + shooter.setState(ShooterState.PASS); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_WITH_GP); } + } + } + + + @Override + public void robotPeriodic() { + super.robotPeriodic(); + + // continous sate action switch (getState()) { case SPEAKER_PREPARE_TO_SCORE -> { shooter.setDistanceToFeedSpot(distanceToFeedSpot); shooter.setDistanceToSpeaker(distanceToSpeaker); } + case FEEDING_PREPARE_TO_SHOOT -> { + shooter.setDistanceToFeedSpot(distanceToFeedSpot); + shooter.setDistanceToSpeaker(distanceToSpeaker); + } + case AMP_PREPARE_TO_SCORE, + PASS_PREPARE_TO_SHOOT->{} } } -} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/shooter/ShooterRpms.java b/src/main/java/frc/robot/shooter/ShooterRpms.java index f39ebdf..6b0e247 100644 --- a/src/main/java/frc/robot/shooter/ShooterRpms.java +++ b/src/main/java/frc/robot/shooter/ShooterRpms.java @@ -5,4 +5,6 @@ public class ShooterRpms { public static final double DROP = 500.0; public static final double PODIUM = 4000.0; public static final double IDLE_WARMUP = 1000.0; + public static final double AMP = 500.0; + public static final double PASS = 2000.0; } diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java index 813f952..2d848a7 100644 --- a/src/main/java/frc/robot/shooter/ShooterState.java +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -7,5 +7,7 @@ public enum ShooterState { DROP, SUBWOOFER_SHOT, FEEDING, - PODIUM_SHOT; + PODIUM_SHOT, + AMP, + PASS; } diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java index 02a7fc0..5fce8a1 100644 --- a/src/main/java/frc/robot/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java @@ -46,6 +46,10 @@ public boolean atGoal() { MathUtil.isNear(feedSpotDistanceToRpm.get(distanceToFeedSpot), shooterRPM, 50); case SPEAKER_SHOT -> MathUtil.isNear(speakerDistanceToRpm.get(distanceToSpeaker), shooterRPM, 50); + case AMP-> + MathUtil.isNear(ShooterRpms.AMP,shooterRPM,50); + case PASS-> + MathUtil.isNear(ShooterRpms.PASS, shooterRPM, 50); }; } @@ -74,6 +78,8 @@ protected void afterTransition(ShooterState newState) { velocityRequest.withVelocity(speakerDistanceToRpm.get(distanceToFeedSpot) / 60.0)); case DROP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); case PODIUM_SHOT -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); + case AMP->motor.setControl(velocityRequest.withVelocity(ShooterRpms.AMP/60.0)); + case PASS-> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS/60.0)); } } From 123fb4f76cb745ab7bedc089851b3b5b2733b4c5 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sat, 7 Sep 2024 11:51:58 -0700 Subject: [PATCH 03/10] spotless --- src/main/java/frc/robot/arm/ArmSubsystem.java | 5 ++- .../frc/robot/robot_manager/RobotManager.java | 37 ++++++++----------- .../java/frc/robot/shooter/ShooterRpms.java | 2 +- .../frc/robot/shooter/ShooterSubsystem.java | 10 ++--- 4 files changed, 23 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 6f0829b..f4767c9 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -93,7 +93,8 @@ public boolean atGoal() { MathUtil.isNear(ArmAngle.AMP.getDegrees(), leftMotorAngle, 1) && MathUtil.isNear(ArmAngle.AMP.getDegrees(), rightMotorAngle, 1); case PASS -> - MathUtil.isNear(ArmAngle.PASS.getDegrees(), leftMotorAngle, 1)&&MathUtil.isNear(ArmAngle.PASS.getDegrees(), rightMotorAngle, 1); + MathUtil.isNear(ArmAngle.PASS.getDegrees(), leftMotorAngle, 1) + && MathUtil.isNear(ArmAngle.PASS.getDegrees(), rightMotorAngle, 1); }; } @@ -157,7 +158,7 @@ protected void afterTransition(ArmState newState) { leftMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations())); rightMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations())); } - case PASS->{ + case PASS -> { leftMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations())); rightMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations())); } diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 189fded..baea2e5 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -43,42 +43,40 @@ public RobotManager( this.intake = intake; this.queuer = queuer; } - + @Override protected void collectInputs() {} - protected RobotState getNexState(RobotState currentState){ + protected RobotState getNexState(RobotState currentState) { // state transition switch (currentState) { case SPEAKER_PREPARE_TO_SCORE -> { - if (shooter.atGoal()&&arm.atGoal()) { + if (shooter.atGoal() && arm.atGoal()) { setStateFromRequest(RobotState.SPEAKER_SCORING); } } - case AMP_PREPARE_TO_SCORE->{ - if (shooter.atGoal()&&arm.atGoal()){ + case AMP_PREPARE_TO_SCORE -> { + if (shooter.atGoal() && arm.atGoal()) { setStateFromRequest(RobotState.AMP_SCORING); } } - case FEEDING_PREPARE_TO_SHOOT ->{ - if (shooter.atGoal()&&arm.atGoal()){ + case FEEDING_PREPARE_TO_SHOOT -> { + if (shooter.atGoal() && arm.atGoal()) { setStateFromRequest(RobotState.FEEDING_SHOOTING); } - } - case PASS_PREPARE_TO_SHOOT->{ - if (shooter.atGoal()&&arm.atGoal()){ + case PASS_PREPARE_TO_SHOOT -> { + if (shooter.atGoal() && arm.atGoal()) { setStateFromRequest(RobotState.PASS_SHOOTING); } } - - } return currentState; } + @Override - protected void afterTransition(RobotState newState){ - // on state change + protected void afterTransition(RobotState newState) { + // on state change switch (newState) { case SPEAKER_PREPARE_TO_SCORE -> { arm.setState(ArmState.SPEAKER_SHOT); @@ -104,15 +102,12 @@ protected void afterTransition(RobotState newState){ intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - - } } - + } @Override public void robotPeriodic() { super.robotPeriodic(); - // continous sate action switch (getState()) { @@ -124,9 +119,7 @@ public void robotPeriodic() { shooter.setDistanceToFeedSpot(distanceToFeedSpot); shooter.setDistanceToSpeaker(distanceToSpeaker); } - case AMP_PREPARE_TO_SCORE, - PASS_PREPARE_TO_SHOOT->{} + case AMP_PREPARE_TO_SCORE, PASS_PREPARE_TO_SHOOT -> {} } } - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/shooter/ShooterRpms.java b/src/main/java/frc/robot/shooter/ShooterRpms.java index 6b0e247..aa60b48 100644 --- a/src/main/java/frc/robot/shooter/ShooterRpms.java +++ b/src/main/java/frc/robot/shooter/ShooterRpms.java @@ -5,6 +5,6 @@ public class ShooterRpms { public static final double DROP = 500.0; public static final double PODIUM = 4000.0; public static final double IDLE_WARMUP = 1000.0; - public static final double AMP = 500.0; + public static final double AMP = 500.0; public static final double PASS = 2000.0; } diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java index 5fce8a1..3274db6 100644 --- a/src/main/java/frc/robot/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java @@ -46,10 +46,8 @@ public boolean atGoal() { MathUtil.isNear(feedSpotDistanceToRpm.get(distanceToFeedSpot), shooterRPM, 50); case SPEAKER_SHOT -> MathUtil.isNear(speakerDistanceToRpm.get(distanceToSpeaker), shooterRPM, 50); - case AMP-> - MathUtil.isNear(ShooterRpms.AMP,shooterRPM,50); - case PASS-> - MathUtil.isNear(ShooterRpms.PASS, shooterRPM, 50); + case AMP -> MathUtil.isNear(ShooterRpms.AMP, shooterRPM, 50); + case PASS -> MathUtil.isNear(ShooterRpms.PASS, shooterRPM, 50); }; } @@ -78,8 +76,8 @@ protected void afterTransition(ShooterState newState) { velocityRequest.withVelocity(speakerDistanceToRpm.get(distanceToFeedSpot) / 60.0)); case DROP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); case PODIUM_SHOT -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); - case AMP->motor.setControl(velocityRequest.withVelocity(ShooterRpms.AMP/60.0)); - case PASS-> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS/60.0)); + case AMP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.AMP / 60.0)); + case PASS -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS / 60.0)); } } From 5da0feaa8c5d48f5396ad28f880e4c542a923f84 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sat, 7 Sep 2024 13:16:30 -0700 Subject: [PATCH 04/10] continue robot manager --- .../frc/robot/robot_manager/RobotManager.java | 123 ++++++++++++++++-- 1 file changed, 112 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index baea2e5..507d0fa 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -23,8 +23,8 @@ public class RobotManager extends StateMachine { public final IntakeSubsystem intake; public final QueuerSubsystem queuer; - private final double distanceToFeedSpot = 0; - private final double distanceToSpeaker = 0; + private final double distanceToFeedSpot = 0.0; + private final double distanceToSpeaker = 0.0; public RobotManager( ArmSubsystem arm, @@ -50,11 +50,32 @@ protected void collectInputs() {} protected RobotState getNexState(RobotState currentState) { // state transition switch (currentState) { + case SPEAKER_SCORING -> { + if (!queuer.hasNote()) { + setStateFromRequest(RobotState.IDLE_NO_GP); + } + } + case AMP_SCORING -> { + if (!queuer.hasNote()) { + setStateFromRequest(RobotState.IDLE_NO_GP); + } + } + case FEEDING_SHOOTING -> { + if (!queuer.hasNote()) { + setStateFromRequest(RobotState.IDLE_NO_GP); + } + } + case PASS_SHOOTING -> { + if (!queuer.hasNote()) { + setStateFromRequest(RobotState.IDLE_NO_GP); + } + } case SPEAKER_PREPARE_TO_SCORE -> { if (shooter.atGoal() && arm.atGoal()) { setStateFromRequest(RobotState.SPEAKER_SCORING); } } + case AMP_PREPARE_TO_SCORE -> { if (shooter.atGoal() && arm.atGoal()) { setStateFromRequest(RobotState.AMP_SCORING); @@ -70,6 +91,33 @@ protected RobotState getNexState(RobotState currentState) { setStateFromRequest(RobotState.PASS_SHOOTING); } } + case UNJAM -> {} + case INTAKING -> { + if (intake.hasNote() || queuer.hasNote()) { + setStateFromRequest(RobotState.IDLE_WITH_GP); + } + } + case OUTTAKING -> { + if (!intake.hasNote() && !queuer.hasNote()) { + setStateFromRequest(RobotState.IDLE_NO_GP); + } + } + case CLIMBING_1_LINEUP -> { + if (arm.atGoal()) { + setStateFromRequest(RobotState.CLIMBING_2_HANGING); + } + } + case CLIMBING_2_HANGING -> {} + case IDLE_NO_GP -> { + if (queuer.hasNote() || intake.hasNote()) { + setStateFromRequest(RobotState.IDLE_WITH_GP); + } + } + case IDLE_WITH_GP -> { + if (!queuer.hasNote() && !intake.hasNote()) { + setStateFromRequest(RobotState.IDLE_NO_GP); + } + } } return currentState; } @@ -78,30 +126,72 @@ protected RobotState getNexState(RobotState currentState) { protected void afterTransition(RobotState newState) { // on state change switch (newState) { - case SPEAKER_PREPARE_TO_SCORE -> { + case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING -> { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case AMP_PREPARE_TO_SCORE -> { + case AMP_PREPARE_TO_SCORE, AMP_SCORING -> { arm.setState(ArmState.AMP); shooter.setState(ShooterState.AMP); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case FEEDING_PREPARE_TO_SHOOT -> { + case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING -> { arm.setState(ArmState.FEEDING); shooter.setState(ShooterState.FEEDING); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case PASS_PREPARE_TO_SHOOT -> { + case PASS_PREPARE_TO_SHOOT, PASS_SHOOTING -> { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } + case UNJAM -> { + arm.setState(ArmState.AMP); + shooter.setState(ShooterState.DROP); + intake.setState(IntakeState.OUTTAKING); + queuer.seState(QueuerState.OUTTAKING); + } + case INTAKING -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.INTAKING); + queuer.seState(QueuerState.INTAKING); + } + case OUTTAKING -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.OUTTAKING); + queuer.seState(QueuerState.OUTTAKING); + } + case CLIMBING_1_LINEUP -> { + arm.setState(ArmState.CLIMBING_1_LINEUP); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_NO_GP); + } + case CLIMBING_2_HANGING -> { + arm.setState(ArmState.CLIMBING_1_LINEUP); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_NO_GP); + } + case IDLE_NO_GP -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_NO_GP); + } + case IDLE_WITH_GP -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.IDLE_WITH_GP); + } } } @@ -111,15 +201,26 @@ public void robotPeriodic() { // continous sate action switch (getState()) { - case SPEAKER_PREPARE_TO_SCORE -> { - shooter.setDistanceToFeedSpot(distanceToFeedSpot); + case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING -> { shooter.setDistanceToSpeaker(distanceToSpeaker); + arm.setDistanceToSpeaker(distanceToSpeaker); } - case FEEDING_PREPARE_TO_SHOOT -> { + case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING -> { shooter.setDistanceToFeedSpot(distanceToFeedSpot); - shooter.setDistanceToSpeaker(distanceToSpeaker); + arm.setDistanceToFeedSpot(distanceToFeedSpot); } - case AMP_PREPARE_TO_SCORE, PASS_PREPARE_TO_SHOOT -> {} + + case AMP_PREPARE_TO_SCORE, + PASS_PREPARE_TO_SHOOT, + AMP_SCORING, + PASS_SHOOTING, + UNJAM, + INTAKING, + OUTTAKING, + IDLE_NO_GP, + IDLE_WITH_GP, + CLIMBING_1_LINEUP, + CLIMBING_2_HANGING -> {} } } } From fc974bf25ea1538049d3ff3685df63e5dc6037f9 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sat, 7 Sep 2024 19:09:37 -0700 Subject: [PATCH 05/10] state transition change --- .../frc/robot/robot_manager/RobotManager.java | 41 ++++++++----------- 1 file changed, 18 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 507d0fa..7990bc8 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -50,6 +50,14 @@ protected void collectInputs() {} protected RobotState getNexState(RobotState currentState) { // state transition switch (currentState) { + case SPEAKER_WAITING, + AMP_WAITING, + FEEDING_WAITING, + PASS_WAITING, + IDLE_NO_GP, + IDLE_WITH_GP, + CLIMBING_1_LINEUP, + CLIMBING_2_HANGING -> {} case SPEAKER_SCORING -> { if (!queuer.hasNote()) { setStateFromRequest(RobotState.IDLE_NO_GP); @@ -102,23 +110,8 @@ protected RobotState getNexState(RobotState currentState) { setStateFromRequest(RobotState.IDLE_NO_GP); } } - case CLIMBING_1_LINEUP -> { - if (arm.atGoal()) { - setStateFromRequest(RobotState.CLIMBING_2_HANGING); - } - } - case CLIMBING_2_HANGING -> {} - case IDLE_NO_GP -> { - if (queuer.hasNote() || intake.hasNote()) { - setStateFromRequest(RobotState.IDLE_WITH_GP); - } - } - case IDLE_WITH_GP -> { - if (!queuer.hasNote() && !intake.hasNote()) { - setStateFromRequest(RobotState.IDLE_NO_GP); - } - } } + return currentState; } @@ -126,25 +119,25 @@ protected RobotState getNexState(RobotState currentState) { protected void afterTransition(RobotState newState) { // on state change switch (newState) { - case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING -> { + case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case AMP_PREPARE_TO_SCORE, AMP_SCORING -> { + case AMP_PREPARE_TO_SCORE, AMP_SCORING, AMP_WAITING -> { arm.setState(ArmState.AMP); shooter.setState(ShooterState.AMP); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING -> { + case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING, FEEDING_WAITING -> { arm.setState(ArmState.FEEDING); shooter.setState(ShooterState.FEEDING); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case PASS_PREPARE_TO_SHOOT, PASS_SHOOTING -> { + case PASS_PREPARE_TO_SHOOT, PASS_SHOOTING, PASS_WAITING -> { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); @@ -201,11 +194,11 @@ public void robotPeriodic() { // continous sate action switch (getState()) { - case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING -> { + case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> { shooter.setDistanceToSpeaker(distanceToSpeaker); arm.setDistanceToSpeaker(distanceToSpeaker); } - case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING -> { + case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING, FEEDING_WAITING -> { shooter.setDistanceToFeedSpot(distanceToFeedSpot); arm.setDistanceToFeedSpot(distanceToFeedSpot); } @@ -220,7 +213,9 @@ public void robotPeriodic() { IDLE_NO_GP, IDLE_WITH_GP, CLIMBING_1_LINEUP, - CLIMBING_2_HANGING -> {} + CLIMBING_2_HANGING, + PASS_WAITING, + AMP_WAITING -> {} } } } From fe82a7813183f0523505b889bc526759ef03931d Mon Sep 17 00:00:00 2001 From: Jonah Snider Date: Sun, 8 Sep 2024 12:46:35 -0700 Subject: [PATCH 06/10] Fix typos & formatting --- .../frc/robot/robot_manager/RobotManager.java | 22 ++++--------------- 1 file changed, 4 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 7990bc8..b9ac6f7 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -47,8 +47,8 @@ public RobotManager( @Override protected void collectInputs() {} - protected RobotState getNexState(RobotState currentState) { - // state transition + @Override + protected RobotState getNextState(RobotState currentState) { switch (currentState) { case SPEAKER_WAITING, AMP_WAITING, @@ -117,7 +117,6 @@ protected RobotState getNexState(RobotState currentState) { @Override protected void afterTransition(RobotState newState) { - // on state change switch (newState) { case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> { arm.setState(ArmState.SPEAKER_SHOT); @@ -192,7 +191,7 @@ protected void afterTransition(RobotState newState) { public void robotPeriodic() { super.robotPeriodic(); - // continous sate action + // Continuous state actions switch (getState()) { case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> { shooter.setDistanceToSpeaker(distanceToSpeaker); @@ -202,20 +201,7 @@ public void robotPeriodic() { shooter.setDistanceToFeedSpot(distanceToFeedSpot); arm.setDistanceToFeedSpot(distanceToFeedSpot); } - - case AMP_PREPARE_TO_SCORE, - PASS_PREPARE_TO_SHOOT, - AMP_SCORING, - PASS_SHOOTING, - UNJAM, - INTAKING, - OUTTAKING, - IDLE_NO_GP, - IDLE_WITH_GP, - CLIMBING_1_LINEUP, - CLIMBING_2_HANGING, - PASS_WAITING, - AMP_WAITING -> {} + default -> {} } } } From acfed3e888b995ded4e9ecd777018684493ac1c8 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Sun, 8 Sep 2024 14:09:58 -0700 Subject: [PATCH 07/10] robot manager fixes --- .../frc/robot/robot_manager/RobotManager.java | 124 ++++++++---------- 1 file changed, 56 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index b9ac6f7..12f410e 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -49,102 +49,90 @@ protected void collectInputs() {} @Override protected RobotState getNextState(RobotState currentState) { - switch (currentState) { + return switch (currentState) { case SPEAKER_WAITING, - AMP_WAITING, - FEEDING_WAITING, - PASS_WAITING, - IDLE_NO_GP, - IDLE_WITH_GP, - CLIMBING_1_LINEUP, - CLIMBING_2_HANGING -> {} - case SPEAKER_SCORING -> { - if (!queuer.hasNote()) { - setStateFromRequest(RobotState.IDLE_NO_GP); - } - } - case AMP_SCORING -> { - if (!queuer.hasNote()) { - setStateFromRequest(RobotState.IDLE_NO_GP); - } - } - case FEEDING_SHOOTING -> { - if (!queuer.hasNote()) { - setStateFromRequest(RobotState.IDLE_NO_GP); - } - } - case PASS_SHOOTING -> { - if (!queuer.hasNote()) { - setStateFromRequest(RobotState.IDLE_NO_GP); - } - } - case SPEAKER_PREPARE_TO_SCORE -> { - if (shooter.atGoal() && arm.atGoal()) { - setStateFromRequest(RobotState.SPEAKER_SCORING); - } - } + AMP_WAITING, + FEEDING_WAITING, + PASS_WAITING, + IDLE_NO_GP, + IDLE_WITH_GP, + CLIMBING_1_LINEUP, + CLIMBING_2_HANGING -> + currentState; + case SPEAKER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING -> + !queuer.hasNote() ? RobotState.IDLE_NO_GP : currentState; - case AMP_PREPARE_TO_SCORE -> { - if (shooter.atGoal() && arm.atGoal()) { - setStateFromRequest(RobotState.AMP_SCORING); - } - } - case FEEDING_PREPARE_TO_SHOOT -> { - if (shooter.atGoal() && arm.atGoal()) { - setStateFromRequest(RobotState.FEEDING_SHOOTING); - } - } - case PASS_PREPARE_TO_SHOOT -> { - if (shooter.atGoal() && arm.atGoal()) { - setStateFromRequest(RobotState.PASS_SHOOTING); - } - } - case UNJAM -> {} - case INTAKING -> { - if (intake.hasNote() || queuer.hasNote()) { - setStateFromRequest(RobotState.IDLE_WITH_GP); - } - } - case OUTTAKING -> { - if (!intake.hasNote() && !queuer.hasNote()) { - setStateFromRequest(RobotState.IDLE_NO_GP); - } - } - } + case SPEAKER_PREPARE_TO_SCORE -> + shooter.atGoal() && arm.atGoal() ? RobotState.SPEAKER_SCORING : currentState; + + case AMP_PREPARE_TO_SCORE -> + shooter.atGoal() && arm.atGoal() ? RobotState.AMP_SCORING : currentState; - return currentState; + case FEEDING_PREPARE_TO_SHOOT -> + shooter.atGoal() && arm.atGoal() ? RobotState.FEEDING_SHOOTING : currentState; + case PASS_PREPARE_TO_SHOOT -> + shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState; + case UNJAM -> currentState; + case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState; + case OUTTAKING -> + !queuer.hasNote() || !intake.hasNote() ? RobotState.IDLE_NO_GP : currentState; + }; } @Override protected void afterTransition(RobotState newState) { switch (newState) { - case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> { + case SPEAKER_PREPARE_TO_SCORE, SPEAKER_WAITING -> { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case AMP_PREPARE_TO_SCORE, AMP_SCORING, AMP_WAITING -> { + case SPEAKER_SCORING -> { + arm.setState(ArmState.SPEAKER_SHOT); + shooter.setState(ShooterState.SPEAKER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.SHOOTING); + } + case AMP_PREPARE_TO_SCORE, AMP_WAITING -> { arm.setState(ArmState.AMP); shooter.setState(ShooterState.AMP); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING, FEEDING_WAITING -> { + case AMP_SCORING -> { + arm.setState(ArmState.AMP); + shooter.setState(ShooterState.AMP); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.SHOOTING); + } + case FEEDING_PREPARE_TO_SHOOT, FEEDING_WAITING -> { arm.setState(ArmState.FEEDING); shooter.setState(ShooterState.FEEDING); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } - case PASS_PREPARE_TO_SHOOT, PASS_SHOOTING, PASS_WAITING -> { + case FEEDING_SHOOTING -> { + arm.setState(ArmState.FEEDING); + shooter.setState(ShooterState.FEEDING); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.SHOOTING); + } + case PASS_PREPARE_TO_SHOOT, PASS_WAITING -> { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } + case PASS_SHOOTING -> { + arm.setState(ArmState.PASS); + shooter.setState(ShooterState.PASS); + intake.setState(IntakeState.IDLE); + queuer.seState(QueuerState.SHOOTING); + } case UNJAM -> { arm.setState(ArmState.AMP); - shooter.setState(ShooterState.DROP); + shooter.setState(ShooterState.PASS); intake.setState(IntakeState.OUTTAKING); queuer.seState(QueuerState.OUTTAKING); } @@ -167,7 +155,7 @@ protected void afterTransition(RobotState newState) { queuer.seState(QueuerState.IDLE_NO_GP); } case CLIMBING_2_HANGING -> { - arm.setState(ArmState.CLIMBING_1_LINEUP); + arm.setState(ArmState.CLIMBING_2_HANGING); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_NO_GP); @@ -180,7 +168,7 @@ protected void afterTransition(RobotState newState) { } case IDLE_WITH_GP -> { arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); + shooter.setState(ShooterState.IDLE_WARMUP); intake.setState(IntakeState.IDLE); queuer.seState(QueuerState.IDLE_WITH_GP); } From 0c8fea611343a945e5a36f98694f37b3b8568634 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Mon, 9 Sep 2024 18:15:08 -0700 Subject: [PATCH 08/10] small fix and typo --- .../frc/robot/queuer/QueuerSubsystem.java | 2 +- .../frc/robot/robot_manager/RobotManager.java | 34 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/queuer/QueuerSubsystem.java b/src/main/java/frc/robot/queuer/QueuerSubsystem.java index 278bbf9..0d469b8 100644 --- a/src/main/java/frc/robot/queuer/QueuerSubsystem.java +++ b/src/main/java/frc/robot/queuer/QueuerSubsystem.java @@ -24,7 +24,7 @@ public QueuerSubsystem(TalonFX motor, DigitalInput sensor) { motor.getConfigurator().apply(RobotConfig.get().queuer().motorConfig()); } - public void seState(QueuerState newState) { + public void setState(QueuerState newState) { setStateFromRequest(newState); } diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 12f410e..b300e63 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -60,7 +60,7 @@ protected RobotState getNextState(RobotState currentState) { CLIMBING_2_HANGING -> currentState; case SPEAKER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING -> - !queuer.hasNote() ? RobotState.IDLE_NO_GP : currentState; + queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP; case SPEAKER_PREPARE_TO_SCORE -> shooter.atGoal() && arm.atGoal() ? RobotState.SPEAKER_SCORING : currentState; @@ -75,7 +75,7 @@ protected RobotState getNextState(RobotState currentState) { case UNJAM -> currentState; case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState; case OUTTAKING -> - !queuer.hasNote() || !intake.hasNote() ? RobotState.IDLE_NO_GP : currentState; + queuer.hasNote() || intake.hasNote() ? currentState : RobotState.IDLE_NO_GP; }; } @@ -86,91 +86,91 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE_WITH_GP); } case SPEAKER_SCORING -> { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.SHOOTING); + queuer.setState(QueuerState.SHOOTING); } case AMP_PREPARE_TO_SCORE, AMP_WAITING -> { arm.setState(ArmState.AMP); shooter.setState(ShooterState.AMP); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE_WITH_GP); } case AMP_SCORING -> { arm.setState(ArmState.AMP); shooter.setState(ShooterState.AMP); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.SHOOTING); + queuer.setState(QueuerState.SHOOTING); } case FEEDING_PREPARE_TO_SHOOT, FEEDING_WAITING -> { arm.setState(ArmState.FEEDING); shooter.setState(ShooterState.FEEDING); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE_WITH_GP); } case FEEDING_SHOOTING -> { arm.setState(ArmState.FEEDING); shooter.setState(ShooterState.FEEDING); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.SHOOTING); + queuer.setState(QueuerState.SHOOTING); } case PASS_PREPARE_TO_SHOOT, PASS_WAITING -> { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE_WITH_GP); } case PASS_SHOOTING -> { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.SHOOTING); + queuer.setState(QueuerState.SHOOTING); } case UNJAM -> { arm.setState(ArmState.AMP); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.OUTTAKING); - queuer.seState(QueuerState.OUTTAKING); + queuer.setState(QueuerState.OUTTAKING); } case INTAKING -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.INTAKING); - queuer.seState(QueuerState.INTAKING); + queuer.setState(QueuerState.INTAKING); } case OUTTAKING -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.OUTTAKING); - queuer.seState(QueuerState.OUTTAKING); + queuer.setState(QueuerState.OUTTAKING); } case CLIMBING_1_LINEUP -> { arm.setState(ArmState.CLIMBING_1_LINEUP); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_NO_GP); + queuer.setState(QueuerState.IDLE_NO_GP); } case CLIMBING_2_HANGING -> { arm.setState(ArmState.CLIMBING_2_HANGING); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_NO_GP); + queuer.setState(QueuerState.IDLE_NO_GP); } case IDLE_NO_GP -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_NO_GP); + queuer.setState(QueuerState.IDLE_NO_GP); } case IDLE_WITH_GP -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_WARMUP); intake.setState(IntakeState.IDLE); - queuer.seState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE_WITH_GP); } } } From 74bf66638995cf4d9971ac7197801488b55330f4 Mon Sep 17 00:00:00 2001 From: Jonah Snider Date: Mon, 9 Sep 2024 23:30:07 -0700 Subject: [PATCH 09/10] Format with Spotless --- src/main/java/frc/robot/robot_manager/RobotManager.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index b300e63..df5c36c 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -74,8 +74,7 @@ protected RobotState getNextState(RobotState currentState) { shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState; case UNJAM -> currentState; case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState; - case OUTTAKING -> - queuer.hasNote() || intake.hasNote() ? currentState : RobotState.IDLE_NO_GP; + case OUTTAKING -> queuer.hasNote() || intake.hasNote() ? currentState : RobotState.IDLE_NO_GP; }; } From 3fdd5de0cbf770705d40a54b246fe8086b1efd93 Mon Sep 17 00:00:00 2001 From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com> Date: Tue, 10 Sep 2024 17:16:11 -0700 Subject: [PATCH 10/10] robot manager finished --- .../frc/robot/robot_manager/RobotManager.java | 130 +++++++++++++++++- .../frc/robot/robot_manager/RobotState.java | 7 +- 2 files changed, 132 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index df5c36c..b696cbf 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -52,14 +52,14 @@ protected RobotState getNextState(RobotState currentState) { return switch (currentState) { case SPEAKER_WAITING, AMP_WAITING, + SUBWOOFER_WAITING, FEEDING_WAITING, - PASS_WAITING, IDLE_NO_GP, IDLE_WITH_GP, CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> currentState; - case SPEAKER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING -> + case SPEAKER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING, SUBWOOFER_SCORING -> queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP; case SPEAKER_PREPARE_TO_SCORE -> @@ -72,6 +72,8 @@ protected RobotState getNextState(RobotState currentState) { shooter.atGoal() && arm.atGoal() ? RobotState.FEEDING_SHOOTING : currentState; case PASS_PREPARE_TO_SHOOT -> shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState; + case SUBWOOFER_PREPARE_TO_SCORE -> + shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : currentState; case UNJAM -> currentState; case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState; case OUTTAKING -> queuer.hasNote() || intake.hasNote() ? currentState : RobotState.IDLE_NO_GP; @@ -81,6 +83,18 @@ protected RobotState getNextState(RobotState currentState) { @Override protected void afterTransition(RobotState newState) { switch (newState) { + case SUBWOOFER_PREPARE_TO_SCORE, SUBWOOFER_WAITING -> { + arm.setState(ArmState.SUBWOOFER_SHOT); + shooter.setState(ShooterState.SUBWOOFER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE_WITH_GP); + } + case SUBWOOFER_SCORING -> { + arm.setState(ArmState.SUBWOOFER_SHOT); + shooter.setState(ShooterState.SUBWOOFER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.SHOOTING); + } case SPEAKER_PREPARE_TO_SCORE, SPEAKER_WAITING -> { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); @@ -117,7 +131,7 @@ protected void afterTransition(RobotState newState) { intake.setState(IntakeState.IDLE); queuer.setState(QueuerState.SHOOTING); } - case PASS_PREPARE_TO_SHOOT, PASS_WAITING -> { + case PASS_PREPARE_TO_SHOOT -> { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); @@ -191,4 +205,114 @@ public void robotPeriodic() { default -> {} } } + + public void confirmShotRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + + case AMP_WAITING -> setStateFromRequest(RobotState.AMP_PREPARE_TO_SCORE); + case SPEAKER_WAITING -> setStateFromRequest(RobotState.SPEAKER_PREPARE_TO_SCORE); + case FEEDING_WAITING -> setStateFromRequest(RobotState.FEEDING_PREPARE_TO_SHOOT); + default -> setStateFromRequest(RobotState.SPEAKER_PREPARE_TO_SCORE); + } + } + + public void waitAmpRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING, AMP_SCORING -> {} + default -> setStateFromRequest(RobotState.AMP_WAITING); + } + } + + public void waitSubwooferRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING, SPEAKER_SCORING -> {} + default -> setStateFromRequest(RobotState.SUBWOOFER_WAITING); + } + } + + public void waitSpeakerRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.SPEAKER_WAITING); + } + } + + public void unjamRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.UNJAM); + } + } + + public void intakeRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.INTAKING); + } + } + + public void stopIntakingRequest() { + switch (getState()) { + case INTAKING -> setStateFromRequest(RobotState.IDLE_NO_GP); + default -> {} + } + } + + public void outtakeRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.OUTTAKING); + } + } + + public void idleWithGpRequest() { + setStateFromRequest(RobotState.IDLE_WITH_GP); + } + + public void stowRequest() { + switch (getState()) { + case INTAKING, + AMP_PREPARE_TO_SCORE, + SPEAKER_PREPARE_TO_SCORE, + FEEDING_PREPARE_TO_SHOOT, + PASS_PREPARE_TO_SHOOT, + AMP_WAITING, + SPEAKER_WAITING, + FEEDING_WAITING, + AMP_SCORING, + SPEAKER_SCORING, + FEEDING_SHOOTING, + PASS_SHOOTING, + IDLE_WITH_GP, + UNJAM -> + setStateFromRequest(RobotState.IDLE_WITH_GP); + default -> setStateFromRequest(RobotState.IDLE_NO_GP); + } + } + + public void preparePassRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.PASS_PREPARE_TO_SHOOT); + } + } + + public void nextClimbStateRequest() { + switch (getState()) { + case CLIMBING_1_LINEUP -> setStateFromRequest(RobotState.CLIMBING_2_HANGING); + case CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.CLIMBING_1_LINEUP); + } + } + + public void previousClimbStateRequest() { + switch (getState()) { + case CLIMBING_2_HANGING -> setStateFromRequest(RobotState.CLIMBING_1_LINEUP); + case CLIMBING_1_LINEUP -> { + setStateFromRequest(RobotState.IDLE_WITH_GP); + } + default -> setStateFromRequest(RobotState.CLIMBING_1_LINEUP); + } + } } diff --git a/src/main/java/frc/robot/robot_manager/RobotState.java b/src/main/java/frc/robot/robot_manager/RobotState.java index 39cda4f..52baf29 100644 --- a/src/main/java/frc/robot/robot_manager/RobotState.java +++ b/src/main/java/frc/robot/robot_manager/RobotState.java @@ -21,10 +21,13 @@ public enum RobotState { UNJAM, - PASS_WAITING, PASS_PREPARE_TO_SHOOT, PASS_SHOOTING, CLIMBING_1_LINEUP, - CLIMBING_2_HANGING; + CLIMBING_2_HANGING, + + SUBWOOFER_WAITING, + SUBWOOFER_PREPARE_TO_SCORE, + SUBWOOFER_SCORING; }