From 12b0593f6147f9b6f555d57fb53d0a9dde4ee90f Mon Sep 17 00:00:00 2001 From: EpicPigGuy Date: Tue, 4 Feb 2025 18:18:09 -0500 Subject: [PATCH 01/16] command update, some done untouched are skipfornow --- .../commands/robotState/AlgaeScoreCommand.java | 13 +++++++++++++ .../commands/robotState/GetAlgaeCommand.java | 13 +++++++++++++ .../robotState/LowHighAlgaeCommand.java | 18 ++++++++++++++++++ .../commands/robotState/ReefCycleCommand.java | 9 +++++++++ .../robotState/ToggleLeftRightCommand.java | 18 ++++++++++++++++++ .../robotState/RobotStateSubsystem.java | 3 +-- 6 files changed, 72 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java diff --git a/src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java b/src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java new file mode 100644 index 00000000..e3849e8f --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java @@ -0,0 +1,13 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AlgaeScoreCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public AlgaeScoreCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + robotState.releaseAlgae(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java new file mode 100644 index 00000000..329c643f --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java @@ -0,0 +1,13 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class GetAlgaeCommand extends InstantCommand { + RobotStateSubsystem robotState; + public GetAlgaeCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + //skipfornow + } +} diff --git a/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java new file mode 100644 index 00000000..e9b54dde --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.AlgaeHeight; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class LowHighAlgaeCommand extends InstantCommand { + RobotStateSubsystem robotState; + public LowHighAlgaeCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + if(AlgaeHeight.LOW == AlgaeHeight.valueOf(getName())) { + robotState.setAlgaeHeight(AlgaeHeight.HIGH); + } + else{ + robotState.setAlgaeHeight(AlgaeHeight.LOW); + } + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java new file mode 100644 index 00000000..c7accbfc --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java @@ -0,0 +1,9 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ReefCycleCommand extends Command { + //skipfornow +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java new file mode 100644 index 00000000..7c6b19bd --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ToggleLeftRightCommand extends InstantCommand { + RobotStateSubsystem robotState; + public ToggleLeftRightCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + if (ScoreSide.valueOf(getName()) == ScoreSide.LEFT){ + robotState.setScoreSide(ScoreSide.RIGHT); + } + else{ + robotState.setScoreSide(ScoreSide.LEFT); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 912937de..523649a8 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -43,7 +43,6 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private RobotStates curState; private RobotStates nextState; private RobotStates futureState; - private ScoringLevel scoringLevel; private ScoreSide scoreSide; private AlgaeHeight algaeHeight; @@ -638,4 +637,4 @@ public enum CoralLoc { SCORING, NONE } -} +} \ No newline at end of file From f078f0c5c836228b3c630741dd8cb62095cc9904 Mon Sep 17 00:00:00 2001 From: EpicPigGuy Date: Tue, 4 Feb 2025 18:50:23 -0500 Subject: [PATCH 02/16] update added getter for get algae on cycle --- .../robot/commands/robotState/ClimbCommand.java | 15 +++++++++++++++ .../commands/robotState/ClimbPrepCommand.java | 15 +++++++++++++++ .../commands/robotState/GetAlgaeCommand.java | 12 ++++++++++-- .../commands/robotState/HPAlgaeCommand.java | 16 ++++++++++++++++ .../frc/robot/commands/robotState/L1Command.java | 16 ++++++++++++++++ .../frc/robot/commands/robotState/L2Command.java | 16 ++++++++++++++++ .../frc/robot/commands/robotState/L3Command.java | 16 ++++++++++++++++ .../frc/robot/commands/robotState/L4Command.java | 16 ++++++++++++++++ .../commands/robotState/LowHighAlgaeCommand.java | 3 +++ .../robot/commands/robotState/StowCommand.java | 15 +++++++++++++++ .../robotState/ToggleLeftRightCommand.java | 3 +++ .../robotState/RobotStateSubsystem.java | 4 ++++ 12 files changed, 145 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/robotState/ClimbCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/L1Command.java create mode 100644 src/main/java/frc/robot/commands/robotState/L2Command.java create mode 100644 src/main/java/frc/robot/commands/robotState/L3Command.java create mode 100644 src/main/java/frc/robot/commands/robotState/L4Command.java create mode 100644 src/main/java/frc/robot/commands/robotState/StowCommand.java diff --git a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java new file mode 100644 index 00000000..c82925d7 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ClimbCommand extends InstantCommand { + RobotStateSubsystem robotState; + public ClimbCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toClimb(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java new file mode 100644 index 00000000..d320e519 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ClimbPrepCommand extends InstantCommand { + RobotStateSubsystem robotState; + public ClimbPrepCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toPrepClimb(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java index 329c643f..b5ca389b 100644 --- a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java @@ -8,6 +8,14 @@ public class GetAlgaeCommand extends InstantCommand { RobotStateSubsystem robotState; public GetAlgaeCommand(RobotStateSubsystem robotState){ this.robotState = robotState; - //skipfornow } -} + @Override + public void initialize() { + if( == true) { + robotState.setGetAlgaeOnCycle(false); + } + else{ + robotState.setGetAlgaeOnCycle(true); + } + } + } diff --git a/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java new file mode 100644 index 00000000..2b8aa577 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java @@ -0,0 +1,16 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HPAlgaeCommand extends InstantCommand { + RobotStateSubsystem robotState; + public HPAlgaeCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toHpAlgae(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/L1Command.java b/src/main/java/frc/robot/commands/robotState/L1Command.java new file mode 100644 index 00000000..dad274e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/L1Command.java @@ -0,0 +1,16 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class L1Command extends InstantCommand { + RobotStateSubsystem robotState; + public L1Command(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.setScoringLevel(ScoringLevel.L1); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/L2Command.java b/src/main/java/frc/robot/commands/robotState/L2Command.java new file mode 100644 index 00000000..8cb2da3f --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/L2Command.java @@ -0,0 +1,16 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class L2Command extends InstantCommand { + RobotStateSubsystem robotState; + public L2Command(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.setScoringLevel(ScoringLevel.L2); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/L3Command.java b/src/main/java/frc/robot/commands/robotState/L3Command.java new file mode 100644 index 00000000..e4b6e145 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/L3Command.java @@ -0,0 +1,16 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class L3Command extends InstantCommand { + RobotStateSubsystem robotState; + public L3Command(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.setScoringLevel(ScoringLevel.L3); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/L4Command.java b/src/main/java/frc/robot/commands/robotState/L4Command.java new file mode 100644 index 00000000..b7af0469 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/L4Command.java @@ -0,0 +1,16 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class L4Command extends InstantCommand { + RobotStateSubsystem robotState; + public L4Command(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.setScoringLevel(ScoringLevel.L4); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java index e9b54dde..db1fa724 100644 --- a/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java @@ -8,6 +8,9 @@ public class LowHighAlgaeCommand extends InstantCommand { RobotStateSubsystem robotState; public LowHighAlgaeCommand(RobotStateSubsystem robotState){ this.robotState = robotState; + } + @Override + public void initialize() { if(AlgaeHeight.LOW == AlgaeHeight.valueOf(getName())) { robotState.setAlgaeHeight(AlgaeHeight.HIGH); } diff --git a/src/main/java/frc/robot/commands/robotState/StowCommand.java b/src/main/java/frc/robot/commands/robotState/StowCommand.java new file mode 100644 index 00000000..05a28979 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/StowCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class StowCommand extends InstantCommand { + RobotStateSubsystem robotState; + public StowCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toStow(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java index 7c6b19bd..e8b8a510 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java @@ -8,6 +8,9 @@ public class ToggleLeftRightCommand extends InstantCommand { RobotStateSubsystem robotState; public ToggleLeftRightCommand(RobotStateSubsystem robotState){ this.robotState = robotState; + } + @Override + public void initialize() { if (ScoreSide.valueOf(getName()) == ScoreSide.LEFT){ robotState.setScoreSide(ScoreSide.RIGHT); } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 523649a8..e81fa5dd 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -150,6 +150,10 @@ public void setGetAlgaeOnCycle(boolean getAlgaeOnCycle) { this.getAlgaeOnCycle = getAlgaeOnCycle; } + public boolean getGetAlgaeOnCycle(){ + return getAlgaeOnCycle; + } + public void setCurrentLimiting(boolean isCurrentLimiting) { this.isCurrentLimiting = isCurrentLimiting; } From 65a9ba8077edefa25c8bfd4f5a8f74aa8764a7d6 Mon Sep 17 00:00:00 2001 From: EpicPigGuy Date: Tue, 4 Feb 2025 18:57:00 -0500 Subject: [PATCH 03/16] getIsAuto added to RobotState --- .../commands/robotState/GetAlgaeCommand.java | 3 +-- .../commands/robotState/HPAlgaeCommand.java | 1 - .../commands/robotState/ReefCycleCommand.java | 17 ++++++++++++----- .../robotState/RobotStateSubsystem.java | 4 ++++ 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java index b5ca389b..fa64c0fe 100644 --- a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java @@ -1,7 +1,6 @@ package frc.robot.commands.robotState; import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; import edu.wpi.first.wpilibj2.command.InstantCommand; public class GetAlgaeCommand extends InstantCommand { @@ -11,7 +10,7 @@ public GetAlgaeCommand(RobotStateSubsystem robotState){ } @Override public void initialize() { - if( == true) { + if(robotState.getGetAlgaeOnCycle() == true) { robotState.setGetAlgaeOnCycle(false); } else{ diff --git a/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java index 2b8aa577..961e787f 100644 --- a/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java @@ -1,7 +1,6 @@ package frc.robot.commands.robotState; import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import edu.wpi.first.wpilibj2.command.InstantCommand; public class HPAlgaeCommand extends InstantCommand { diff --git a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java index c7accbfc..5a748e6b 100644 --- a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java @@ -1,9 +1,16 @@ package frc.robot.commands.robotState; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ReefCycleCommand extends Command { - //skipfornow -} \ No newline at end of file +public class ReefCycleCommand extends InstantCommand { + RobotStateSubsystem robotState; + public ReefCycleCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState. + } +} diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index e81fa5dd..a7193368 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -162,6 +162,10 @@ public void setIsAuto(boolean isAuto) { this.isAuto = isAuto; } + public boolean getIsAuto(){ + return isAuto; + } + private boolean needSafeAlgaeTransfer(RobotStates nextState) { if (algaeSubsystem.hasAlgae()) { switch (curState) { From 693ade781480078972e15b9dcc04d53aaef3a14b Mon Sep 17 00:00:00 2001 From: EpicPigGuy Date: Tue, 4 Feb 2025 19:22:01 -0500 Subject: [PATCH 04/16] almost final, must add reef cycle --- ...cleCommand.java => AutoVsManualCommand.java} | 12 ++++++++---- .../DriveCommands/AlgaeScoreCommand.java | 15 +++++++++++++++ .../{ => DriveCommands}/ClimbCommand.java | 2 +- .../DriveCommands/FloorAlgaeCommand.java | 15 +++++++++++++++ .../DriveCommands/InteruptAutoCommand.java | 17 +++++++++++++++++ 5 files changed, 56 insertions(+), 5 deletions(-) rename src/main/java/frc/robot/commands/robotState/{ReefCycleCommand.java => AutoVsManualCommand.java} (50%) create mode 100644 src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java rename src/main/java/frc/robot/commands/robotState/{ => DriveCommands}/ClimbCommand.java (87%) create mode 100644 src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java diff --git a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/AutoVsManualCommand.java similarity index 50% rename from src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java rename to src/main/java/frc/robot/commands/robotState/AutoVsManualCommand.java index 5a748e6b..8d44f72a 100644 --- a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/AutoVsManualCommand.java @@ -1,16 +1,20 @@ package frc.robot.commands.robotState; import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ReefCycleCommand extends InstantCommand { +public class AutoVsManualCommand extends InstantCommand { RobotStateSubsystem robotState; - public ReefCycleCommand(RobotStateSubsystem robotState){ + public AutoVsManualCommand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override public void initialize() { - robotState. + if (robotState.getIsAuto() == false){ + robotState.setIsAuto(true); + } + else{ + robotState.setIsAuto(false); + } } } diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java new file mode 100644 index 00000000..6161cccb --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands.robotState.DriveCommands; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AlgaeScoreCommand extends InstantCommand { + RobotStateSubsystem robotState; + public AlgaeScoreCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toScoreAlgae(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/ClimbCommand.java similarity index 87% rename from src/main/java/frc/robot/commands/robotState/ClimbCommand.java rename to src/main/java/frc/robot/commands/robotState/DriveCommands/ClimbCommand.java index c82925d7..46c9ae73 100644 --- a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/ClimbCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.robotState; +package frc.robot.commands.robotState.DriveCommands; import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java new file mode 100644 index 00000000..677baa6b --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands.robotState.DriveCommands; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class FloorAlgaeCommand extends InstantCommand { + RobotStateSubsystem robotState; + public FloorAlgaeCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toAlgaeFloorPickup(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java new file mode 100644 index 00000000..15b7e280 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.robotState.DriveCommands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class InteruptAutoCommand extends InstantCommand{ + RobotStateSubsystem robotState; + + public InteruptAutoCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toInterrupted(); + } +} + From 96999e7078e09dae88229601f38288122f14a618 Mon Sep 17 00:00:00 2001 From: EpicPigGuy Date: Mon, 10 Feb 2025 18:14:42 -0500 Subject: [PATCH 05/16] reef cycle update --- .../DriveCommands/reefCycleCommand.java | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java new file mode 100644 index 00000000..174dbd2f --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java @@ -0,0 +1,15 @@ +package frc.robot.commands.robotState.DriveCommands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class reefCycleCommand extends InstantCommand { + RobotStateSubsystem robotState; + public reefCycleCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.toReefAlign(); + } +} From 64b03e39174e9f87c53aa2e959aed73470123e36 Mon Sep 17 00:00:00 2001 From: EpicPigGuy Date: Mon, 10 Feb 2025 18:17:17 -0500 Subject: [PATCH 06/16] capitialize --- .../commands/robotState/DriveCommands/reefCycleCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java index 174dbd2f..576f5a74 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java @@ -3,9 +3,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; -public class reefCycleCommand extends InstantCommand { +public class ReefCycleCommand extends InstantCommand { RobotStateSubsystem robotState; - public reefCycleCommand(RobotStateSubsystem robotState){ + public ReefCycleCommand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override From 6810f63ce47c256b02edfab242f38fc88f9027d7 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 11 Feb 2025 21:06:59 -0500 Subject: [PATCH 07/16] did some stuff --- src/main/java/frc/robot/RobotContainer.java | 27 +++++--- .../commands/biscuit/HoldBiscuitCommand.java | 18 +++++ .../commands/biscuit/JogBiscuitCommand.java | 32 +++++++++ .../robotState/ToggleGetAlgaeCommand.java | 17 +++++ .../frc/robot/constants/BiscuitConstants.java | 40 +++++++---- .../robot/subsystems/biscuit/BiscuitIOFX.java | 14 +++- .../robotState/RobotStateSubsystem.java | 31 +++++---- ...atest.json => Phoenix6-replay-25.2.2.json} | 66 +++++++++---------- vendordeps/thirdcoast.json | 4 +- 9 files changed, 176 insertions(+), 73 deletions(-) create mode 100644 src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java create mode 100644 src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java rename vendordeps/{Phoenix6-replay-frc2025-latest.json => Phoenix6-replay-25.2.2.json} (92%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c9d9469..b0737fa1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,17 +14,19 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.algae.OpenLoopAlgaeCommand; +import frc.robot.commands.biscuit.HoldBiscuitCommand; +import frc.robot.commands.biscuit.JogBiscuitCommand; import frc.robot.commands.coral.EnableEjectBeamCommand; import frc.robot.commands.coral.OpenLoopCoralCommand; import frc.robot.commands.drive.DriveTeleopCommand; import frc.robot.commands.drive.ResetGyroCommand; -import frc.robot.commands.elevator.HoldElevatorCommand; -import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; import frc.robot.commands.robotState.ScoreReefManualCommand; import frc.robot.commands.robotState.SetScoringLevelCommand; import frc.robot.commands.robotState.StowCommand; +import frc.robot.commands.robotState.ToggleGetAlgaeCommand; +import frc.robot.constants.BiscuitConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotConstants; import frc.robot.controllers.FlyskyJoystick; @@ -141,7 +143,7 @@ public RobotContainer() { configureTelemetry(); configureDriverBindings(); - configureOperatorBindings(); + configureTestOperatorBindings(); } private void configureTelemetry() { @@ -150,6 +152,7 @@ private void configureTelemetry() { algaeSubsystem.registerWith(telemetryService); elevatorSubsystem.registerWith(telemetryService); funnelSubsystem.registerWith(telemetryService); + biscuitSubsystem.registerWith(telemetryService); telemetryService.start(); } @@ -182,8 +185,12 @@ private void configureOperatorBindings() { robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); // zero elevator, slated for removal - new JoystickButton(xboxController, XboxController.Button.kX.value) + new JoystickButton(xboxController, XboxController.Button.kY.value) .onTrue(new ZeroElevatorCommand(elevatorSubsystem)); + + // Algae + new JoystickButton(xboxController, XboxController.Button.kX.value) + .onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem)); } private void configureTestOperatorBindings() { @@ -204,14 +211,14 @@ private void configureTestOperatorBindings() { // Move Elevator new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); // Zero Elevator new JoystickButton(xboxController, XboxController.Button.kX.value) diff --git a/src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java b/src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java new file mode 100644 index 00000000..10ddb00e --- /dev/null +++ b/src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.biscuit; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; + +public class HoldBiscuitCommand extends InstantCommand { + private BiscuitSubsystem biscuitSubsystem; + + public HoldBiscuitCommand(BiscuitSubsystem biscuitSubsystem) { + this.biscuitSubsystem = biscuitSubsystem; + addRequirements(biscuitSubsystem); + } + + @Override + public void initialize() { + biscuitSubsystem.setPosition(biscuitSubsystem.getPosition()); + } +} diff --git a/src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java b/src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java new file mode 100644 index 00000000..fa73d747 --- /dev/null +++ b/src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.biscuit; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; + +public class JogBiscuitCommand extends Command { + + private BiscuitSubsystem biscuitSubsystem; + private Angle positionChange; + + public JogBiscuitCommand(BiscuitSubsystem biscuitSubsystem, Angle positionChange) { + this.biscuitSubsystem = biscuitSubsystem; + this.positionChange = positionChange; + addRequirements(biscuitSubsystem); + } + + @Override + public void initialize() { + biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange)); + } + + @Override + public void execute() { + biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange)); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java new file mode 100644 index 00000000..7f397221 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ToggleGetAlgaeCommand extends InstantCommand { + RobotStateSubsystem robotStateSubsystem; + + public ToggleGetAlgaeCommand(RobotStateSubsystem robotStateSubsystem) { + this.robotStateSubsystem = robotStateSubsystem; + } + + @Override + public void initialize() { + robotStateSubsystem.ToggleGetAlgaeOnCycle(); + } +} diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 7c3e76fa..e67dac84 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -2,7 +2,9 @@ import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.configs.CommutationConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -10,9 +12,11 @@ import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; @@ -21,7 +25,8 @@ public class BiscuitConstants { // These are all wrong right now because we don't have any actual info - public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined + public static double kZero = 0.02; // TODO Will need to be experimentally determined + public static double kTicksPerRot = 160; public static int talonID = 25; public static double kCloseEnough = 0.05; // This was a little out of wack. public static final Angle kMaxFwd = Rotations.of(100); @@ -58,6 +63,10 @@ public class BiscuitConstants { public static final Angle kProcessorSetpoint = Rotations.of(0.0); public static final Angle kBargeSetpoint = Rotations.of(0.0); + // jogging + public static final double kJogAmountUp = 1; + public static final double kJogAmountDown = 1; + // Disables the TalonFXS by setting it's voltage to zero. Not very shocking. public static VoltageConfigs disableTalon() { VoltageConfigs voltage = @@ -73,10 +82,9 @@ public static TalonFXSConfiguration getFXSConfig() { new CurrentLimitsConfigs() .withStatorCurrentLimit(10) .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(20) - .withSupplyCurrentLimit(10) - .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimit(20) + .withSupplyCurrentLowerLimit(5) + .withSupplyCurrentLowerTime(2) .withSupplyCurrentLimitEnable(true); fxsConfig.CurrentLimits = current; @@ -94,31 +102,31 @@ public static TalonFXSConfiguration getFXSConfig() { SoftwareLimitSwitchConfigs swLimit = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) + .withForwardSoftLimitEnable(false) .withForwardSoftLimitThreshold(kMaxFwd) - .withReverseSoftLimitEnable(true) + .withReverseSoftLimitEnable(false) .withReverseSoftLimitThreshold(kMaxRev); fxsConfig.SoftwareLimitSwitch = swLimit; Slot0Configs slot0 = new Slot0Configs() - .withKP(0) + .withKP(2) .withKI(0) .withKD(0) .withGravityType(GravityTypeValue.Elevator_Static) .withKG(0) - .withKS(0) + .withKS(1) .withKV(0) .withKA(0); fxsConfig.Slot0 = slot0; MotionMagicConfigs motionMagic = new MotionMagicConfigs() - .withMotionMagicAcceleration(0) - .withMotionMagicCruiseVelocity(0) + .withMotionMagicAcceleration(500) + .withMotionMagicCruiseVelocity(100) .withMotionMagicExpo_kA(0) .withMotionMagicExpo_kV(0) - .withMotionMagicJerk(0); + .withMotionMagicJerk(1000); fxsConfig.MotionMagic = motionMagic; MotorOutputConfigs motorOut = @@ -127,6 +135,14 @@ public static TalonFXSConfiguration getFXSConfig() { .withNeutralMode(NeutralModeValue.Coast); fxsConfig.MotorOutput = motorOut; + CommutationConfigs commutation = + new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST); + fxsConfig.Commutation = commutation; + + ExternalFeedbackConfigs feedBack = + new ExternalFeedbackConfigs() + .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + return fxsConfig; } } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index 1d456f50..5e687a3c 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Alert; @@ -26,6 +27,8 @@ public class BiscuitIOFX implements BiscuitIO { private StatusSignal position; private StatusSignal velocity; private StatusSignal fwdLimitSwitch; + private StatusSignal rawQuadrature; + private StatusSignal rawPulseWidth; private boolean didZero; private boolean fwdLimitSwitchOpen; private Angle offset; @@ -51,6 +54,11 @@ public BiscuitIOFX() { // Set our variables velocity = talon.getVelocity(); position = talon.getPosition(); + rawQuadrature = talon.getRawQuadraturePosition(); + rawQuadrature.setUpdateFrequency(20); + rawPulseWidth = talon.getRawPulseWidthPosition(); + rawPulseWidth.setUpdateFrequency(20); + zero(); } @Override @@ -76,8 +84,10 @@ public void registerWith(TelemetryService telemetry) { public void zero() { didZero = false; if (fwdLimitSwitchOpen == true) { - Angle pos = position.getValue(); - offset = BiscuitConstants.kZero.minus(pos); + double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1); + double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos); + talon.setPosition(setPos); + logger.info("set Biscuit position to " + setPos); didZero = true; } else { rangeAlert.set(true); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index ab663ad0..f9d1c45c 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -174,6 +174,10 @@ public void setAutoPlacingLed(boolean isAutoPlacing) { ledSubsystem.setAutoPlacing(isAutoPlacing); } + public void ToggleGetAlgaeOnCycle() { + getAlgaeOnCycle = !getAlgaeOnCycle; + } + private boolean needSafeAlgaeTransfer(RobotStates nextState) { // if (algaeSubsystem.hasAlgae()) { // switch (curState) { @@ -221,7 +225,7 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) { } public void toStow() { - // biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); // algaeSubsystem.hold(); @@ -241,7 +245,7 @@ public void toPrepCoral() { } private void toFunnelLoad() { - // biscuitSubsystem.setPosition(BiscuitConstants.kFunnelSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kFunnelSetpoint); coralSubsystem.intake(); elevatorSubsystem.setPosition(ElevatorConstants.kFunnelSetpoint); @@ -253,7 +257,10 @@ public void toReefAlign() { } private void toReefAlign(boolean getAlgae, boolean drive) { - if (getAlgae) { + if (!coralSubsystem.hasCoral()) { + getAlgae = true; + } + if (getAlgae && scoreSide == ScoreSide.LEFT) { if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) { return; } @@ -285,19 +292,19 @@ private void toReefAlign(boolean getAlgae, boolean drive) { currentLevel = scoringLevel; switch (scoringLevel) { case L1 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); } case L2 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); } case L3 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); } case L4 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); } } @@ -447,9 +454,7 @@ public void periodic() { switch (curState) { case TRANSFER -> { - if ( - // biscuitSubsystem.isFinished() && - elevatorSubsystem.isFinished() + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished() // && climbSubsystem.isFinished() ) { setState(nextState); @@ -457,9 +462,7 @@ public void periodic() { } case TO_STOW -> { - if ( - // biscuitSubsystem.isFinished() && - elevatorSubsystem.isFinished()) { + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { setState(RobotStates.STOW); } } @@ -583,7 +586,7 @@ public void periodic() { case LOADING_CORAL -> { if (coralSubsystem.hasCoral()) { coralLoc = CoralLoc.CORAL; - // biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint); funnelSubsystem.stopMotor(); diff --git a/vendordeps/Phoenix6-replay-frc2025-latest.json b/vendordeps/Phoenix6-replay-25.2.2.json similarity index 92% rename from vendordeps/Phoenix6-replay-frc2025-latest.json rename to vendordeps/Phoenix6-replay-25.2.2.json index f2ff57eb..f94c1084 100644 --- a/vendordeps/Phoenix6-replay-frc2025-latest.json +++ b/vendordeps/Phoenix6-replay-25.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-replay-frc2025-latest.json", + "fileName": "Phoenix6-replay-25.2.2.json", "name": "CTRE-Phoenix (v6) Replay", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,7 +48,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "api-cpp-replay", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -62,7 +62,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +76,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,7 +118,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -132,7 +132,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -146,7 +146,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -160,7 +160,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -174,7 +174,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -202,7 +202,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -216,7 +216,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -232,7 +232,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -245,7 +245,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "wpiapi-cpp-replay", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPIReplay", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Replay", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -418,7 +418,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -434,7 +434,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -450,7 +450,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index d142a4ca..975916e8 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "25.0.1", + "version": "25.0.2", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "25.0.1" + "version": "25.0.2" } ], "jniDependencies": [], From efa6633acc4299d479b5a331c901093b44b09b27 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 13 Feb 2025 20:13:02 -0500 Subject: [PATCH 08/16] got jogging of biscuit working, filled out a bunch of setpoints --- .vscode/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 16 ++++++- .../frc/robot/constants/BiscuitConstants.java | 42 ++++++++++--------- .../robot/constants/ElevatorConstants.java | 22 +++++----- .../robot/subsystems/biscuit/BiscuitIOFX.java | 3 +- .../subsystems/biscuit/BiscuitSubsystem.java | 2 +- 6 files changed, 51 insertions(+), 36 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 9a299ae5..446902b5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b0737fa1..a5f2928b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,8 @@ import frc.robot.commands.coral.OpenLoopCoralCommand; import frc.robot.commands.drive.DriveTeleopCommand; import frc.robot.commands.drive.ResetGyroCommand; +import frc.robot.commands.elevator.HoldElevatorCommand; +import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; import frc.robot.commands.robotState.ScoreReefManualCommand; @@ -208,7 +210,7 @@ private void configureTestOperatorBindings() { .onTrue(new OpenLoopCoralCommand(coralSubsystem, 1)) .onTrue(new EnableEjectBeamCommand(false, coralSubsystem)); - // Move Elevator + // Move biscuit new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) .onTrue( new JogBiscuitCommand( @@ -220,6 +222,18 @@ private void configureTestOperatorBindings() { biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + // Move elevator + new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + // Zero Elevator new JoystickButton(xboxController, XboxController.Button.kX.value) .onTrue(new ZeroElevatorCommand(elevatorSubsystem)); diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index e67dac84..03a406fd 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -35,37 +35,38 @@ public class BiscuitConstants { // Setpoints // Idle public static final Angle kStowSetpoint = Rotations.of(0.0); - public static final Angle kFunnelSetpoint = Rotations.of(0.0); - public static final Angle kPrestageSetpoint = Rotations.of(0.0); + public static final Angle kFunnelSetpoint = kStowSetpoint; + public static final Angle kPrestageSetpoint = kStowSetpoint; // Algae removal - public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeSetpoint = Rotations.of(120.12604); + public static final Angle kL3AlgaeSetpoint = Rotations.of(120.12604); - public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(120.12604); + public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(120.12604); public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); // Coral score - public static final Angle kL1CoralSetpoint = Rotations.of(0.0); - public static final Angle kL2CoralSetpoint = Rotations.of(0.0); - public static final Angle kL3CoralSetpoint = Rotations.of(0.0); - public static final Angle kL4CoralSetpoint = Rotations.of(0.0); + public static final Angle kL1CoralSetpoint = kStowSetpoint; + public static final Angle kL2CoralSetpoint = kStowSetpoint; + public static final Angle kL3CoralSetpoint = kStowSetpoint; + public static final Angle kL4CoralSetpoint = kStowSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(320.74); public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0); - public static final Angle kHpAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kHpAlgaeSetpoint = Rotations.of(106.6608); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(0.0); - public static final Angle kBargeSetpoint = Rotations.of(0.0); + public static final Angle kProcessorSetpoint = Rotations.of(319.6908); + public static final Angle kBargeSetpoint = Rotations.of(77.5906); + public static final Angle kBargeBackwardsSetpoint = Rotations.of(-77.5906); // jogging - public static final double kJogAmountUp = 1; - public static final double kJogAmountDown = 1; + public static final double kJogAmountUp = 10; + public static final double kJogAmountDown = -10; // Disables the TalonFXS by setting it's voltage to zero. Not very shocking. public static VoltageConfigs disableTalon() { @@ -80,7 +81,7 @@ public static TalonFXSConfiguration getFXSConfig() { CurrentLimitsConfigs current = new CurrentLimitsConfigs() - .withStatorCurrentLimit(10) + .withStatorCurrentLimit(0) .withStatorCurrentLimitEnable(false) .withSupplyCurrentLimit(20) .withSupplyCurrentLowerLimit(5) @@ -115,8 +116,8 @@ public static TalonFXSConfiguration getFXSConfig() { .withKD(0) .withGravityType(GravityTypeValue.Elevator_Static) .withKG(0) - .withKS(1) - .withKV(0) + .withKS(0) + .withKV(0.1) .withKA(0); fxsConfig.Slot0 = slot0; @@ -132,7 +133,7 @@ public static TalonFXSConfiguration getFXSConfig() { MotorOutputConfigs motorOut = new MotorOutputConfigs() .withDutyCycleNeutralDeadband(0.01) - .withNeutralMode(NeutralModeValue.Coast); + .withNeutralMode(NeutralModeValue.Brake); fxsConfig.MotorOutput = motorOut; CommutationConfigs commutation = @@ -142,6 +143,7 @@ public static TalonFXSConfiguration getFXSConfig() { ExternalFeedbackConfigs feedBack = new ExternalFeedbackConfigs() .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + fxsConfig.ExternalFeedback = feedBack; return fxsConfig; } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index cfd304a6..fcea3fb7 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -21,7 +21,7 @@ public class ElevatorConstants { public static final double kCloseEnoughRotations = 0.1; - public static final double kMaxFwd = 53; // TODO all of these fields need to be filled out + public static final double kMaxFwd = 53; public static final double kMaxRev = 2; public static final int kZeroMultiple = 0; // some constant to multiply, add by to turn the analog input into a position @@ -43,14 +43,14 @@ public class ElevatorConstants { public static final Angle kStowSetpoint = kFunnelSetpoint; // Algae removal - public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeSetpoint = Rotations.of(2.8677); + public static final Angle kL3AlgaeSetpoint = Rotations.of(2.8677); - public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(2.8677); + public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(2.8677); - public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); + public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(2.8677); + public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(2.8677); // Coral score public static final Angle kL1CoralSetpoint = Rotations.of(13.04053); @@ -62,13 +62,13 @@ public class ElevatorConstants { public static final Angle kPrestageSetpoint = kL3CoralSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(8.81836); public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0); - public static final Angle kHpAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(0.0); - public static final Angle kBargeSetpoint = Rotations.of(0.0); + public static final Angle kProcessorSetpoint = Rotations.of(14.9063); + public static final Angle kBargeSetpoint = Rotations.of(40.913); public static TalonFXConfiguration getBothFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index 5e687a3c..a38b5475 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -68,11 +68,10 @@ public void setPosition(Angle position) { @Override public void updateInputs(BiscuitIOInputs inputs) { - BaseStatusSignal.refreshAll(velocity, position, fwdLimitSwitch); inputs.velocity = velocity.getValue(); inputs.position = position.getValue(); - inputs.fwdLimitSwitchOpen = fwdLimitSwitch.getValueAsDouble() == 1; inputs.didZero = didZero; + BaseStatusSignal.refreshAll(velocity, position); } @Override diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java index 16121b23..cb98dba4 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java @@ -51,7 +51,7 @@ public boolean isFinished() { @Override public void periodic() { - // io.updateInputs(inputs); + io.updateInputs(inputs); Logger.processInputs(getName(), inputs); Logger.recordOutput("Biscuit setPoint", setPoint); Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0); From 30a7e3ffe2040140ab92c8399b1987322b1fb1fe Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 13 Feb 2025 20:22:36 -0500 Subject: [PATCH 09/16] fixing some wierd cap errors --- .../{reefCycleCommand.java => ReefCycleComand.java} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename src/main/java/frc/robot/commands/robotState/DriveCommands/{reefCycleCommand.java => ReefCycleComand.java} (74%) diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleComand.java similarity index 74% rename from src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java rename to src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleComand.java index 576f5a74..c34a645b 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/reefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleComand.java @@ -3,9 +3,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; -public class ReefCycleCommand extends InstantCommand { +public class ReefCycleComand extends InstantCommand { RobotStateSubsystem robotState; - public ReefCycleCommand(RobotStateSubsystem robotState){ + public ReefCycleComand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override From 844b04843707c26f2e7abf4afd686c4946198969 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 13 Feb 2025 20:23:12 -0500 Subject: [PATCH 10/16] part 2 of previous --- .../{ReefCycleComand.java => ReefCycleCommand.java} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename src/main/java/frc/robot/commands/robotState/DriveCommands/{ReefCycleComand.java => ReefCycleCommand.java} (74%) diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleComand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleCommand.java similarity index 74% rename from src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleComand.java rename to src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleCommand.java index c34a645b..576f5a74 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleComand.java +++ b/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleCommand.java @@ -3,9 +3,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; -public class ReefCycleComand extends InstantCommand { +public class ReefCycleCommand extends InstantCommand { RobotStateSubsystem robotState; - public ReefCycleComand(RobotStateSubsystem robotState){ + public ReefCycleCommand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override From 318ede255156baea994b2caeeb1b8285dc84a537 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 13 Feb 2025 21:01:49 -0500 Subject: [PATCH 11/16] mapped many commands to buttons and fixed problems with the commands. still need to fix many of them --- src/main/java/frc/robot/RobotContainer.java | 43 ++++++++++++++++--- .../{DriveCommands => }/ClimbCommand.java | 2 +- .../DriveCommands/AlgaeScoreCommand.java | 15 ------- .../FloorAlgaeCommand.java | 2 +- .../InteruptAutoCommand.java | 2 +- .../{DriveCommands => }/ReefCycleCommand.java | 2 +- ...oreCommand.java => ScoreAlgaeCommand.java} | 6 +-- ...ightCommand.java => ScoreLeftCommand.java} | 11 ++--- .../robotState/ScoreRightCommand.java | 16 +++++++ ...and.java => ToggleAlgaeHeightCommand.java} | 11 ++--- ...ualCommand.java => ToggleAutoCommand.java} | 4 +- .../robotState/RobotStateSubsystem.java | 12 ++++-- 12 files changed, 77 insertions(+), 49 deletions(-) rename src/main/java/frc/robot/commands/robotState/{DriveCommands => }/ClimbCommand.java (87%) delete mode 100644 src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java rename src/main/java/frc/robot/commands/robotState/{DriveCommands => }/FloorAlgaeCommand.java (88%) rename src/main/java/frc/robot/commands/robotState/{DriveCommands => }/InteruptAutoCommand.java (88%) rename src/main/java/frc/robot/commands/robotState/{DriveCommands => }/ReefCycleCommand.java (88%) rename src/main/java/frc/robot/commands/robotState/{AlgaeScoreCommand.java => ScoreAlgaeCommand.java} (60%) rename src/main/java/frc/robot/commands/robotState/{ToggleLeftRightCommand.java => ScoreLeftCommand.java} (52%) create mode 100644 src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java rename src/main/java/frc/robot/commands/robotState/{LowHighAlgaeCommand.java => ToggleAlgaeHeightCommand.java} (51%) rename src/main/java/frc/robot/commands/robotState/{AutoVsManualCommand.java => ToggleAutoCommand.java} (78%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5f2928b..ab0f4721 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,7 +24,15 @@ import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; +import frc.robot.commands.robotState.ToggleAlgaeHeightCommand; +import frc.robot.commands.robotState.ToggleAutoCommand; +import frc.robot.commands.robotState.GetAlgaeCommand; +import frc.robot.commands.robotState.HPAlgaeCommand; +import frc.robot.commands.robotState.InteruptAutoCommand; +import frc.robot.commands.robotState.ScoreAlgaeCommand; +import frc.robot.commands.robotState.ScoreLeftCommand; import frc.robot.commands.robotState.ScoreReefManualCommand; +import frc.robot.commands.robotState.ScoreRightCommand; import frc.robot.commands.robotState.SetScoringLevelCommand; import frc.robot.commands.robotState.StowCommand; import frc.robot.commands.robotState.ToggleGetAlgaeCommand; @@ -163,10 +171,26 @@ private void configureDriverBindings() { new DriveTeleopCommand( () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem)); - // Reset Gyro Command + // Reset Gyro Command, stow, and interrupt Auton new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem)); + new JoystickButton(driveJoystick, Button.SWD.id) + .onTrue(new StowCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)) + .onFalse(new StowCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); + new JoystickButton(driveJoystick, Button.SWA.id) + .onTrue(new InteruptAutoCommand(robotStateSubsystem)) + .onFalse(new InteruptAutoCommand(robotStateSubsystem)); + + // other stuff new JoystickButton(driveJoystick, Button.M_SWH.id) .onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem)); + new JoystickButton(driveJoystick, Button.M_SWE.id) + .onTrue(new ScoreAlgaeCommand(robotStateSubsystem)); + new JoystickButton(driveJoystick, Button.SWF_UP.id) + .onTrue(new GetAlgaeCommand(robotStateSubsystem)) + .onFalse(new GetAlgaeCommand(robotStateSubsystem)); + new JoystickButton(driveJoystick, Button.SWF_DWN.id) + .onTrue(new GetAlgaeCommand(robotStateSubsystem)) + .onFalse(new GetAlgaeCommand(robotStateSubsystem)); } private void configureOperatorBindings() { @@ -186,13 +210,22 @@ private void configureOperatorBindings() { new StowCommand( robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); - // zero elevator, slated for removal - new JoystickButton(xboxController, XboxController.Button.kY.value) - .onTrue(new ZeroElevatorCommand(elevatorSubsystem)); - // Algae + new JoystickButton(xboxController, XboxController.Button.kY.value) + .onTrue(new ToggleAlgaeHeightCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kX.value) .onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kB.value) + .onTrue(new HPAlgaeCommand(robotStateSubsystem)); + + // Scoring + new JoystickButton(xboxController, XboxController.Button.kA.value) + .onTrue(new ToggleAutoCommand(robotStateSubsystem)); + + new JoystickButton(xboxController, XboxController.Button.kRightStick.value) + .onTrue(new ScoreRightCommand(robotStateSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kLeftStick.value) + .onTrue(new ScoreLeftCommand(robotStateSubsystem)); } private void configureTestOperatorBindings() { diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/ClimbCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java similarity index 87% rename from src/main/java/frc/robot/commands/robotState/DriveCommands/ClimbCommand.java rename to src/main/java/frc/robot/commands/robotState/ClimbCommand.java index 46c9ae73..c82925d7 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/ClimbCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.robotState.DriveCommands; +package frc.robot.commands.robotState; import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java b/src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java deleted file mode 100644 index 6161cccb..00000000 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/AlgaeScoreCommand.java +++ /dev/null @@ -1,15 +0,0 @@ -package frc.robot.commands.robotState.DriveCommands; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class AlgaeScoreCommand extends InstantCommand { - RobotStateSubsystem robotState; - public AlgaeScoreCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toScoreAlgae(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java similarity index 88% rename from src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java rename to src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java index 677baa6b..5dc5027b 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/FloorAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.robotState.DriveCommands; +package frc.robot.commands.robotState; import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java similarity index 88% rename from src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java rename to src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java index 15b7e280..72c61033 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/InteruptAutoCommand.java +++ b/src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.robotState.DriveCommands; +package frc.robot.commands.robotState; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; diff --git a/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java similarity index 88% rename from src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleCommand.java rename to src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java index 576f5a74..2ce1662d 100644 --- a/src/main/java/frc/robot/commands/robotState/DriveCommands/ReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.robotState.DriveCommands; +package frc.robot.commands.robotState; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; diff --git a/src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java similarity index 60% rename from src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java rename to src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java index e3849e8f..49817a73 100644 --- a/src/main/java/frc/robot/commands/robotState/AlgaeScoreCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java @@ -3,11 +3,11 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class AlgaeScoreCommand extends InstantCommand { +public class ScoreAlgaeCommand extends InstantCommand { RobotStateSubsystem robotState; - public AlgaeScoreCommand(RobotStateSubsystem robotState){ + public ScoreAlgaeCommand(RobotStateSubsystem robotState){ this.robotState = robotState; - robotState.releaseAlgae(); + robotState.toScoreAlgae(); } } diff --git a/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java similarity index 52% rename from src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java rename to src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java index e8b8a510..045925d3 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleLeftRightCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java @@ -4,18 +4,13 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ToggleLeftRightCommand extends InstantCommand { +public class ScoreLeftCommand extends InstantCommand { RobotStateSubsystem robotState; - public ToggleLeftRightCommand(RobotStateSubsystem robotState){ + public ScoreLeftCommand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override public void initialize() { - if (ScoreSide.valueOf(getName()) == ScoreSide.LEFT){ - robotState.setScoreSide(ScoreSide.RIGHT); - } - else{ - robotState.setScoreSide(ScoreSide.LEFT); - } + robotState.setScoreSide(ScoreSide.LEFT); } } diff --git a/src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java new file mode 100644 index 00000000..d139cfb9 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java @@ -0,0 +1,16 @@ +package frc.robot.commands.robotState; + +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ScoreRightCommand extends InstantCommand { + RobotStateSubsystem robotState; + public ScoreRightCommand(RobotStateSubsystem robotState){ + this.robotState = robotState; + } + @Override + public void initialize() { + robotState.setScoreSide(ScoreSide.RIGHT); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java similarity index 51% rename from src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java rename to src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java index db1fa724..5c1d4b4d 100644 --- a/src/main/java/frc/robot/commands/robotState/LowHighAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java @@ -4,18 +4,13 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem.AlgaeHeight; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class LowHighAlgaeCommand extends InstantCommand { +public class ToggleAlgaeHeightCommand extends InstantCommand { RobotStateSubsystem robotState; - public LowHighAlgaeCommand(RobotStateSubsystem robotState){ + public ToggleAlgaeHeightCommand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override public void initialize() { - if(AlgaeHeight.LOW == AlgaeHeight.valueOf(getName())) { - robotState.setAlgaeHeight(AlgaeHeight.HIGH); - } - else{ - robotState.setAlgaeHeight(AlgaeHeight.LOW); - } + robotState.toggleAlgaeHeight(null); } } diff --git a/src/main/java/frc/robot/commands/robotState/AutoVsManualCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java similarity index 78% rename from src/main/java/frc/robot/commands/robotState/AutoVsManualCommand.java rename to src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java index 8d44f72a..0824f6ae 100644 --- a/src/main/java/frc/robot/commands/robotState/AutoVsManualCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java @@ -3,9 +3,9 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class AutoVsManualCommand extends InstantCommand { +public class ToggleAutoCommand extends InstantCommand { RobotStateSubsystem robotState; - public AutoVsManualCommand(RobotStateSubsystem robotState){ + public ToggleAutoCommand(RobotStateSubsystem robotState){ this.robotState = robotState; } @Override diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 78048d10..ac0a54ad 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -154,6 +154,10 @@ public void setAlgaeHeight(AlgaeHeight algaeHeight) { this.algaeHeight = algaeHeight; } + public void toggleAlgaeHeight(AlgaeHeight algaeHeight) { + this.algaeHeight = algaeHeight == AlgaeHeight.LOW ? AlgaeHeight.HIGH : AlgaeHeight.LOW; + } + public void setAutoPlacing(boolean isAutoPlacing) { this.isAutoPlacing = isAutoPlacing; } @@ -166,6 +170,10 @@ public boolean getGetAlgaeOnCycle(){ return getAlgaeOnCycle; } + public void ToggleGetAlgaeOnCycle() { + getAlgaeOnCycle = !getAlgaeOnCycle; + } + public void setCurrentLimiting(boolean isCurrentLimiting) { this.isCurrentLimiting = isCurrentLimiting; } @@ -182,10 +190,6 @@ public void setAutoPlacingLed(boolean isAutoPlacing) { ledSubsystem.setAutoPlacing(isAutoPlacing); } - public void ToggleGetAlgaeOnCycle() { - getAlgaeOnCycle = !getAlgaeOnCycle; - } - private boolean needSafeAlgaeTransfer(RobotStates nextState) { // if (algaeSubsystem.hasAlgae()) { // switch (curState) { From 3537a7d957003bca74179dca4468d85faea44c22 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Sat, 15 Feb 2025 16:08:01 -0500 Subject: [PATCH 12/16] fixed many commands, fixed constants, and many other wonderous things --- src/main/java/frc/robot/RobotContainer.java | 72 +++++++++----- .../commands/robotState/ClimbCommand.java | 22 +++-- .../commands/robotState/ClimbPrepCommand.java | 20 ++-- .../robotState/FloorAlgaeCommand.java | 42 +++++++-- .../commands/robotState/GetAlgaeCommand.java | 29 +++--- .../commands/robotState/HPAlgaeCommand.java | 39 ++++++-- .../robotState/InterruptAutoCommand.java | 18 ++++ .../robotState/InteruptAutoCommand.java | 17 ---- .../robot/commands/robotState/L1Command.java | 16 ---- .../robot/commands/robotState/L2Command.java | 16 ---- .../robot/commands/robotState/L3Command.java | 16 ---- .../robot/commands/robotState/L4Command.java | 16 ---- .../commands/robotState/ReefCycleCommand.java | 56 +++++++++-- .../robotState/ScoreAlgaeCommand.java | 43 +++++++-- .../commands/robotState/ScoreLeftCommand.java | 16 ---- .../robotState/ScoreRightCommand.java | 16 ---- .../robotState/SetScoreSideRightCommand.java | 18 ++++ .../robotState/ToggleAlgaeHeightCommand.java | 21 +++-- .../robotState/ToggleAutoCommand.java | 27 +++--- .../robotState/setScoreSideLeftCommand.java | 18 ++++ .../frc/robot/constants/BiscuitConstants.java | 2 +- .../frc/robot/constants/DriveConstants.java | 1 + .../robot/constants/ElevatorConstants.java | 2 +- .../frc/robot/subsystems/algae/AlgaeIO.java | 3 +- .../frc/robot/subsystems/algae/AlgaeIOFX.java | 23 ++--- .../subsystems/algae/AlgaeSubsystem.java | 19 ++-- .../subsystems/drive/DriveSubsystem.java | 16 +++- .../robotState/RobotStateSubsystem.java | 94 ++++++++++--------- 28 files changed, 403 insertions(+), 295 deletions(-) create mode 100644 src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java delete mode 100644 src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java delete mode 100644 src/main/java/frc/robot/commands/robotState/L1Command.java delete mode 100644 src/main/java/frc/robot/commands/robotState/L2Command.java delete mode 100644 src/main/java/frc/robot/commands/robotState/L3Command.java delete mode 100644 src/main/java/frc/robot/commands/robotState/L4Command.java delete mode 100644 src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java delete mode 100644 src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab0f4721..da4ade47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,18 +24,18 @@ import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; -import frc.robot.commands.robotState.ToggleAlgaeHeightCommand; -import frc.robot.commands.robotState.ToggleAutoCommand; -import frc.robot.commands.robotState.GetAlgaeCommand; +import frc.robot.commands.robotState.FloorAlgaeCommand; import frc.robot.commands.robotState.HPAlgaeCommand; -import frc.robot.commands.robotState.InteruptAutoCommand; +import frc.robot.commands.robotState.InterruptAutoCommand; +import frc.robot.commands.robotState.ReefCycleCommand; import frc.robot.commands.robotState.ScoreAlgaeCommand; -import frc.robot.commands.robotState.ScoreLeftCommand; -import frc.robot.commands.robotState.ScoreReefManualCommand; -import frc.robot.commands.robotState.ScoreRightCommand; +import frc.robot.commands.robotState.SetScoreSideRightCommand; import frc.robot.commands.robotState.SetScoringLevelCommand; import frc.robot.commands.robotState.StowCommand; +import frc.robot.commands.robotState.ToggleAlgaeHeightCommand; +import frc.robot.commands.robotState.ToggleAutoCommand; import frc.robot.commands.robotState.ToggleGetAlgaeCommand; +import frc.robot.commands.robotState.setScoreSideLeftCommand; import frc.robot.constants.BiscuitConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotConstants; @@ -153,7 +153,7 @@ public RobotContainer() { configureTelemetry(); configureDriverBindings(); - configureTestOperatorBindings(); + configureOperatorBindings(); } private void configureTelemetry() { @@ -171,26 +171,52 @@ private void configureDriverBindings() { new DriveTeleopCommand( () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem)); - // Reset Gyro Command, stow, and interrupt Auton + // Reset Gyro Command, stow, interrupt Auton, and zero elev new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem)); new JoystickButton(driveJoystick, Button.SWD.id) - .onTrue(new StowCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)) - .onFalse(new StowCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); + .onTrue( + new StowCommand( + robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)) + .onFalse( + new StowCommand( + robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); new JoystickButton(driveJoystick, Button.SWA.id) - .onTrue(new InteruptAutoCommand(robotStateSubsystem)) - .onFalse(new InteruptAutoCommand(robotStateSubsystem)); + .onTrue(new InterruptAutoCommand(robotStateSubsystem)) + .onFalse(new InterruptAutoCommand(robotStateSubsystem)); + new JoystickButton(driveJoystick, Button.SWB_UP.id) + .onTrue(new ZeroElevatorCommand(elevatorSubsystem)) + .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); + new JoystickButton(driveJoystick, Button.SWB_DWN.id) + .onTrue(new ZeroElevatorCommand(elevatorSubsystem)) + .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); // other stuff new JoystickButton(driveJoystick, Button.M_SWH.id) - .onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem)); + .onTrue( + new ReefCycleCommand( + robotStateSubsystem, + elevatorSubsystem, + coralSubsystem, + biscuitSubsystem, + algaeSubsystem)); new JoystickButton(driveJoystick, Button.M_SWE.id) - .onTrue(new ScoreAlgaeCommand(robotStateSubsystem)); + .onTrue( + new ScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); new JoystickButton(driveJoystick, Button.SWF_UP.id) - .onTrue(new GetAlgaeCommand(robotStateSubsystem)) - .onFalse(new GetAlgaeCommand(robotStateSubsystem)); + .onTrue( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)) + .onFalse( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); new JoystickButton(driveJoystick, Button.SWF_DWN.id) - .onTrue(new GetAlgaeCommand(robotStateSubsystem)) - .onFalse(new GetAlgaeCommand(robotStateSubsystem)); + .onTrue( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)) + .onFalse( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); } private void configureOperatorBindings() { @@ -216,16 +242,18 @@ private void configureOperatorBindings() { new JoystickButton(xboxController, XboxController.Button.kX.value) .onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kB.value) - .onTrue(new HPAlgaeCommand(robotStateSubsystem)); + .onTrue( + new HPAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); // Scoring new JoystickButton(xboxController, XboxController.Button.kA.value) .onTrue(new ToggleAutoCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kRightStick.value) - .onTrue(new ScoreRightCommand(robotStateSubsystem)); + .onTrue(new SetScoreSideRightCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kLeftStick.value) - .onTrue(new ScoreLeftCommand(robotStateSubsystem)); + .onTrue(new setScoreSideLeftCommand(robotStateSubsystem)); } private void configureTestOperatorBindings() { diff --git a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java index c82925d7..ba1609a5 100644 --- a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java @@ -1,15 +1,17 @@ package frc.robot.commands.robotState; -import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; public class ClimbCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ClimbCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toClimb(); - } -} \ No newline at end of file + RobotStateSubsystem robotState; + + public ClimbCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toClimb(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java index d320e519..bf400e9e 100644 --- a/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java @@ -1,15 +1,17 @@ package frc.robot.commands.robotState; -import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; public class ClimbPrepCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ClimbPrepCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toPrepClimb(); - } + RobotStateSubsystem robotState; + + public ClimbPrepCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toPrepClimb(); + } } diff --git a/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java index 5dc5027b..cf0e8c4b 100644 --- a/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java @@ -1,15 +1,37 @@ package frc.robot.commands.robotState; -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; -public class FloorAlgaeCommand extends InstantCommand { - RobotStateSubsystem robotState; - public FloorAlgaeCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toAlgaeFloorPickup(); +public class FloorAlgaeCommand extends Command { + RobotStateSubsystem robotState; + boolean hasTriedToPickup = false; + + public FloorAlgaeCommand( + RobotStateSubsystem robotState, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotState = robotState; + addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + hasTriedToPickup = false; + robotState.toAlgaeFloorPickup(); + } + + @Override + public boolean isFinished() { + if (robotState.getState() == RobotStates.FLOOR_ALGAE + || robotState.getState() == RobotStates.MIC_ALGAE) { + hasTriedToPickup = true; } + return robotState.getState() == RobotStates.STOW && hasTriedToPickup; + } } diff --git a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java index fa64c0fe..f27d52e2 100644 --- a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java @@ -1,20 +1,21 @@ package frc.robot.commands.robotState; -import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; public class GetAlgaeCommand extends InstantCommand { - RobotStateSubsystem robotState; - public GetAlgaeCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - if(robotState.getGetAlgaeOnCycle() == true) { - robotState.setGetAlgaeOnCycle(false); - } - else{ - robotState.setGetAlgaeOnCycle(true); - } - } + RobotStateSubsystem robotState; + + public GetAlgaeCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + if (robotState.getGetAlgaeOnCycle() == true) { + robotState.setGetAlgaeOnCycle(false); + } else { + robotState.setGetAlgaeOnCycle(true); } + } +} diff --git a/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java index 961e787f..6f25e4c0 100644 --- a/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java @@ -1,15 +1,36 @@ package frc.robot.commands.robotState; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; -public class HPAlgaeCommand extends InstantCommand { - RobotStateSubsystem robotState; - public HPAlgaeCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toHpAlgae(); +public class HPAlgaeCommand extends Command { + RobotStateSubsystem robotState; + private boolean hasTriedToGrab = false; + + public HPAlgaeCommand( + RobotStateSubsystem robotState, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotState = robotState; + addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + hasTriedToGrab = false; + robotState.toHpAlgae(); + } + + @Override + public boolean isFinished() { + if (robotState.getState() == RobotStates.HP_ALGAE) { + hasTriedToGrab = true; } + return robotState.getState() == RobotStates.STOW && hasTriedToGrab; + } } diff --git a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java new file mode 100644 index 00000000..d7790eb2 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class InterruptAutoCommand + extends InstantCommand { // TODO fix this command, it dose not work + RobotStateSubsystem robotState; + + public InterruptAutoCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toInterrupted(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java deleted file mode 100644 index 72c61033..00000000 --- a/src/main/java/frc/robot/commands/robotState/InteruptAutoCommand.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot.commands.robotState; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.subsystems.robotState.RobotStateSubsystem; - -public class InteruptAutoCommand extends InstantCommand{ - RobotStateSubsystem robotState; - - public InteruptAutoCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toInterrupted(); - } -} - diff --git a/src/main/java/frc/robot/commands/robotState/L1Command.java b/src/main/java/frc/robot/commands/robotState/L1Command.java deleted file mode 100644 index dad274e8..00000000 --- a/src/main/java/frc/robot/commands/robotState/L1Command.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.robotState; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class L1Command extends InstantCommand { - RobotStateSubsystem robotState; - public L1Command(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.setScoringLevel(ScoringLevel.L1); - } -} diff --git a/src/main/java/frc/robot/commands/robotState/L2Command.java b/src/main/java/frc/robot/commands/robotState/L2Command.java deleted file mode 100644 index 8cb2da3f..00000000 --- a/src/main/java/frc/robot/commands/robotState/L2Command.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.robotState; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class L2Command extends InstantCommand { - RobotStateSubsystem robotState; - public L2Command(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.setScoringLevel(ScoringLevel.L2); - } -} diff --git a/src/main/java/frc/robot/commands/robotState/L3Command.java b/src/main/java/frc/robot/commands/robotState/L3Command.java deleted file mode 100644 index e4b6e145..00000000 --- a/src/main/java/frc/robot/commands/robotState/L3Command.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.robotState; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class L3Command extends InstantCommand { - RobotStateSubsystem robotState; - public L3Command(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.setScoringLevel(ScoringLevel.L3); - } -} diff --git a/src/main/java/frc/robot/commands/robotState/L4Command.java b/src/main/java/frc/robot/commands/robotState/L4Command.java deleted file mode 100644 index b7af0469..00000000 --- a/src/main/java/frc/robot/commands/robotState/L4Command.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.robotState; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class L4Command extends InstantCommand { - RobotStateSubsystem robotState; - public L4Command(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.setScoringLevel(ScoringLevel.L4); - } -} diff --git a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java index 2ce1662d..1ea8acea 100644 --- a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java @@ -1,15 +1,53 @@ package frc.robot.commands.robotState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; -public class ReefCycleCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ReefCycleCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toReefAlign(); +public class ReefCycleCommand extends Command { + private RobotStateSubsystem robotStateSubsystem; + private ElevatorSubsystem elevatorSubsystem; + private RobotStates startingRobotState; + private boolean startingElevatorFinished; + private boolean isAutoPlacing; + + public ReefCycleCommand( + RobotStateSubsystem robotStateSubsystem, + ElevatorSubsystem elevatorSubsystem, + CoralSubsystem coralSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotStateSubsystem = robotStateSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + startingRobotState = robotStateSubsystem.getState(); + startingElevatorFinished = elevatorSubsystem.isFinished(); + isAutoPlacing = robotStateSubsystem.getIsAuto(); + robotStateSubsystem.toPrepCoral(); + } + + @Override + public boolean isFinished() { + if (isAutoPlacing) { + return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD; + } else { + if (startingRobotState == RobotStates.PRESTAGE || startingRobotState == RobotStates.STOW) { + return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL; + } + if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) { + return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD + || robotStateSubsystem.getState() == RobotStates.LOADING_CORAL + || !startingElevatorFinished; + } } + return false; + } } diff --git a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java index 49817a73..cf919934 100644 --- a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java @@ -1,13 +1,44 @@ package frc.robot.commands.robotState; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; -public class ScoreAlgaeCommand extends InstantCommand { - RobotStateSubsystem robotState; +public class ScoreAlgaeCommand extends Command { + private RobotStateSubsystem robotStateSubsystem; + private ElevatorSubsystem elevatorSubsystem; + private RobotStates startState; + private boolean startingElevatorFinished; - public ScoreAlgaeCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - robotState.toScoreAlgae(); + public ScoreAlgaeCommand( + RobotStateSubsystem robotStateSubsystem, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotStateSubsystem = robotStateSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + startState = robotStateSubsystem.getState(); + startingElevatorFinished = elevatorSubsystem.isFinished(); + robotStateSubsystem.toScoreAlgae(); + } + + @Override + public boolean isFinished() { + if (startState == RobotStates.BARGE_ALGAE || startState == RobotStates.PROCESSOR_ALGAE) { + return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD + || robotStateSubsystem.getState() == RobotStates.STOW + || !startingElevatorFinished; + } else { + return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE + || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE; } + } } diff --git a/src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java deleted file mode 100644 index 045925d3..00000000 --- a/src/main/java/frc/robot/commands/robotState/ScoreLeftCommand.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.robotState; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ScoreLeftCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ScoreLeftCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.setScoreSide(ScoreSide.LEFT); - } -} diff --git a/src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java deleted file mode 100644 index d139cfb9..00000000 --- a/src/main/java/frc/robot/commands/robotState/ScoreRightCommand.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.robotState; - -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ScoreRightCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ScoreRightCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.setScoreSide(ScoreSide.RIGHT); - } -} diff --git a/src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java b/src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java new file mode 100644 index 00000000..f35a8259 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; + +public class SetScoreSideRightCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public SetScoreSideRightCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.setScoreSide(ScoreSide.RIGHT); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java index 5c1d4b4d..44dd4410 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java @@ -1,16 +1,17 @@ package frc.robot.commands.robotState; -import frc.robot.subsystems.robotState.RobotStateSubsystem; -import frc.robot.subsystems.robotState.RobotStateSubsystem.AlgaeHeight; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; public class ToggleAlgaeHeightCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ToggleAlgaeHeightCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - robotState.toggleAlgaeHeight(null); - } + RobotStateSubsystem robotState; + + public ToggleAlgaeHeightCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toggleAlgaeHeight(null); + } } diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java index 0824f6ae..f65b3dd5 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java @@ -1,20 +1,21 @@ package frc.robot.commands.robotState; -import frc.robot.subsystems.robotState.RobotStateSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; public class ToggleAutoCommand extends InstantCommand { - RobotStateSubsystem robotState; - public ToggleAutoCommand(RobotStateSubsystem robotState){ - this.robotState = robotState; - } - @Override - public void initialize() { - if (robotState.getIsAuto() == false){ - robotState.setIsAuto(true); - } - else{ - robotState.setIsAuto(false); - } + RobotStateSubsystem robotState; + + public ToggleAutoCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + if (robotState.getIsAuto() == false) { + robotState.setIsAuto(true); + } else { + robotState.setIsAuto(false); } + } } diff --git a/src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java b/src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java new file mode 100644 index 00000000..3acbf9d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; + +public class setScoreSideLeftCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public setScoreSideLeftCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.setScoreSide(ScoreSide.LEFT); + } +} diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 03a406fd..0fa714bf 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -56,7 +56,7 @@ public class BiscuitConstants { // Algae obtaining public static final Angle kFloorAlgaeSetpoint = Rotations.of(320.74); - public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kMicAlgaeSetpoint = Rotations.of(324.33); public static final Angle kHpAlgaeSetpoint = Rotations.of(106.6608); // Algae scoring diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index a97b3763..20542ac3 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -14,6 +14,7 @@ public class DriveConstants { public static final double kAlgaeRemovalSpeed = 0; + public static final double kBargeScoreStickMultiplier = 0.5; public static final double kDeadbandAllStick = 0.075; public static final double kExpoScaleYawFactor = 0.75; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index fcea3fb7..53ec4664 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -63,7 +63,7 @@ public class ElevatorConstants { // Algae obtaining public static final Angle kFloorAlgaeSetpoint = Rotations.of(8.81836); - public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kMicAlgaeSetpoint = Rotations.of(17.708); public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063); // Algae scoring diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java index 826c63c0..52a70bd4 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -8,8 +8,7 @@ public interface AlgaeIO { @AutoLog public static class AlgaeIOInputs { public AngularVelocity velocity; - public boolean isFwdLimitSwitchClosed; - public boolean isRevLimitSwitchClosed; + public boolean isLimitSwitchClosed; } public default void updateInputs(AlgaeIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java index c99b9aff..6ead7a35 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.algae; -import com.ctre.phoenix6.BaseStatusSignal; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -8,7 +9,6 @@ import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.ReverseLimitValue; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.constants.AlgaeConstants; import org.slf4j.Logger; import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; @@ -27,18 +27,19 @@ public class AlgaeIOFX implements AlgaeIO { public AlgaeIOFX() { logger = LoggerFactory.getLogger(this.getClass()); - talonFX = new TalonFX(AlgaeConstants.kFxId); - fwdLimitSwitch = talonFX.getForwardLimit(); - revLimitSwitch = talonFX.getReverseLimit(); - curVelocity = talonFX.getVelocity(); + // talonFX = new TalonFX(AlgaeConstants.kFxId); + // fwdLimitSwitch = talonFX.getForwardLimit(); + // revLimitSwitch = talonFX.getReverseLimit(); + // curVelocity = talonFX.getVelocity(); } @Override public void updateInputs(AlgaeIOInputs inputs) { - BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); - inputs.velocity = curVelocity.refresh().getValue(); - inputs.isFwdLimitSwitchClosed = fwdLimitSwitch.getValue().value == 1; // FIXME check right value - inputs.isRevLimitSwitchClosed = revLimitSwitch.getValue().value == 0; // FIXME check right value + // BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); + // inputs.velocity = curVelocity.refresh().getValue(); + // inputs.isLimitSwitchClosed = fwdLimitSwitch.getValue().value == 1; // FIXME check right value + inputs.velocity = RotationsPerSecond.of(0); + inputs.isLimitSwitchClosed = true; } @Override @@ -57,6 +58,6 @@ public AngularVelocity AngularVelocity() { @Override public void registerWith(TelemetryService telemetryService) { - telemetryService.register(talonFX, true); + // telemetryService.register(talonFX, true); } } diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java index 58d8f65a..ed06e32e 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -51,14 +51,18 @@ public void hold() { setSpeed(AlgaeConstants.kHoldSpeed); } + public boolean hasAlgae() { + return curState == AlgaeStates.HAS_ALGAE; + } + @Override public void setSpeed(AngularVelocity speed) { - io.setSpeed(speed); - desiredSpeed = speed; + // io.setSpeed(speed); + // desiredSpeed = speed; } public void setPct(double pct) { - io.setPct(pct); + // io.setPct(pct); } @Override @@ -80,13 +84,13 @@ public void periodic() { switch (curState) { case HAS_ALGAE -> { - if (!inputs.isFwdLimitSwitchClosed) { + if (!inputs.isLimitSwitchClosed) { setState(AlgaeStates.EMPTY); setSpeed(RotationsPerSecond.of(0)); } } case EMPTY -> { - if (inputs.isRevLimitSwitchClosed) { // FIXME: correct? + if (inputs.isLimitSwitchClosed) { // FIXME: correct? hold(); setState(AlgaeStates.HAS_ALGAE); } @@ -111,9 +115,4 @@ public enum AlgaeStates { EMPTY, IDLE } - - public boolean hasAlgae() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'hasAlgae'"); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index da8d9f67..df9c8f45 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -40,6 +40,8 @@ public class DriveSubsystem extends MeasurableSubsystem { private Trajectory autoTrajectory; private double trajectoryActive = 0.0; + private double driveMultiplier = 1.0; + private int gyroDifferentCount = 0; private RobotStateSubsystem robotStateSubsystem; @@ -77,7 +79,7 @@ public DriveSubsystem(SwerveIO io) { // Open-Loop Swerve Movements public void drive(double vXmps, double vYmps, double vOmegaRadps) { - io.drive(vXmps, vYmps, vOmegaRadps, true); + io.drive(vXmps * driveMultiplier, vYmps * driveMultiplier, vOmegaRadps, true); } public void setAzimuthVel(double vel) { @@ -319,6 +321,18 @@ public PIDController getomegaControllerNonProfiled() { omegaController.getP(), omegaController.getI(), omegaController.getD()); } + public void setDriveMultiplier(double multiplier) { + driveMultiplier = multiplier; + } + + public double getDriveMultiplier() { + return driveMultiplier; + } + + public void removeDriveMultiplier() { + driveMultiplier = 1.0; + } + @Override public void periodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index ac0a54ad..5c095048 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -166,7 +166,7 @@ public void setGetAlgaeOnCycle(boolean getAlgaeOnCycle) { this.getAlgaeOnCycle = getAlgaeOnCycle; } - public boolean getGetAlgaeOnCycle(){ + public boolean getGetAlgaeOnCycle() { return getAlgaeOnCycle; } @@ -182,7 +182,7 @@ public void setIsAuto(boolean isAuto) { this.isAuto = isAuto; } - public boolean getIsAuto(){ + public boolean getIsAuto() { return isAuto; } @@ -191,47 +191,47 @@ public void setAutoPlacingLed(boolean isAutoPlacing) { } private boolean needSafeAlgaeTransfer(RobotStates nextState) { - // if (algaeSubsystem.hasAlgae()) { - // switch (curState) { - // case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, INTERRUPTED -> { - // switch (nextState) { - // // These are all the states that could possibly be entered that are also possibly - // // dangerous - // // Each futureState must be handled in STOW - // case REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, PREP_CLIMB -> { - // futureState = nextState; - // toStow(); - // return true; - // } - // default -> {} - // } - // } - // default -> {} - // } - - // switch (curState) { - // case REEF_ALIGN_ALGAE, - // REEF_ALIGN_CORAL, - // REMOVE_ALGAE, - // SAFE_REMOVE_ALGAE_ABOVE, - // SAFE_REMOVE_ALGAE_ROTATE, - // PLACE_CORAL, - // INTERRUPTED -> { - // switch (nextState) { - // // These are all the states that could possibly be entered that are also - // // dangerous - // // Each futureState must be handled in STOW - // case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, PREP_CLIMB -> { - // futureState = nextState; - // toStow(); - // return true; - // } - // default -> {} - // } - // } - // default -> {} - // } - // } + if (algaeSubsystem.hasAlgae()) { + switch (curState) { + case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, INTERRUPTED -> { + switch (nextState) { + // These are all the states that could possibly be entered that are also possibly + // dangerous + // Each futureState must be handled in STOW + case REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, PREP_CLIMB -> { + futureState = nextState; + toStow(); + return true; + } + default -> {} + } + } + default -> {} + } + + switch (curState) { + case REEF_ALIGN_ALGAE, + REEF_ALIGN_CORAL, + REMOVE_ALGAE, + SAFE_REMOVE_ALGAE_ABOVE, + SAFE_REMOVE_ALGAE_ROTATE, + PLACE_CORAL, + INTERRUPTED -> { + switch (nextState) { + // These are all the states that could possibly be entered that are also + // dangerous + // Each futureState must be handled in STOW + case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, PREP_CLIMB -> { + futureState = nextState; + toStow(); + return true; + } + default -> {} + } + } + default -> {} + } + } return false; } @@ -239,6 +239,7 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) { public void toStow() { biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); // algaeSubsystem.hold(); setState(RobotStates.TO_STOW); @@ -365,6 +366,9 @@ public void toAlgaeFloorPickup() { } public void toScoreAlgae() { + if (curState == RobotStates.BARGE_ALGAE || curState == RobotStates.PROCESSOR_ALGAE) { + toStow(); + } switch (algaeHeight) { case LOW -> { toProcessor(); @@ -374,6 +378,8 @@ public void toScoreAlgae() { return; } + driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); + double poseX = driveSubsystem.getPoseMeters().getX(); if (poseX > RobotStateConstants.kRedBargeSafeX @@ -698,4 +704,4 @@ public enum CoralLoc { SCORING, NONE } -} \ No newline at end of file +} From 915c089333058cb64de2495aa10bd0593a87c6b4 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 17 Feb 2025 21:01:17 -0500 Subject: [PATCH 13/16] made proggress, do not use the algae --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 13 +- .../commands/algae/ToggleHasAlgaeCommand.java | 19 + .../commands/robotState/GetAlgaeCommand.java | 21 - .../robotState/ToggleAlgaeHeightCommand.java | 2 +- .../frc/robot/constants/BiscuitConstants.java | 22 +- .../frc/robot/constants/DriveConstants.java | 16 +- .../robot/constants/TagServoingConstants.java | 27 +- .../frc/robot/constants/VisionConstants.java | 65 ++- .../subsystems/algae/AlgaeSubsystem.java | 18 +- .../robot/subsystems/biscuit/BiscuitIO.java | 8 +- .../robot/subsystems/biscuit/BiscuitIOFX.java | 4 +- .../subsystems/biscuit/BiscuitSubsystem.java | 14 +- .../subsystems/drive/DriveSubsystem.java | 27 + .../robotState/RobotStateSubsystem.java | 72 ++- .../tagAlign/TagAlignSubsystem.java | 149 ++++-- .../subsystems/vision/VisionSubsystem.java | 477 +++++++++++++++++- 17 files changed, 800 insertions(+), 156 deletions(-) create mode 100644 src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java delete mode 100644 src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java diff --git a/build.gradle b/build.gradle index 3677b87b..f6d13581 100644 --- a/build.gradle +++ b/build.gradle @@ -81,7 +81,7 @@ dependencies { implementation 'ch.qos.logback:logback-classic:1.3.5' //logging implementation("com.opencsv:opencsv:5.6") - implementation 'org.strykeforce:wallEYE:25.0.0' + implementation 'org.strykeforce:wallEYE:25.0.1' implementation 'net.jafama:jafama:2.3.2' //fastMath } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index da4ade47..81166bfc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,11 +9,13 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.algae.OpenLoopAlgaeCommand; +import frc.robot.commands.algae.ToggleHasAlgaeCommand; import frc.robot.commands.biscuit.HoldBiscuitCommand; import frc.robot.commands.biscuit.JogBiscuitCommand; import frc.robot.commands.coral.EnableEjectBeamCommand; @@ -131,7 +133,7 @@ public RobotContainer() { ledIO = new LEDIO(); ledSubsystem = new LEDSubsystem(); - visionSubsystem = new VisionSubsystem(); + visionSubsystem = new VisionSubsystem(driveSubsystem); tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem); @@ -154,6 +156,7 @@ public RobotContainer() { configureTelemetry(); configureDriverBindings(); configureOperatorBindings(); + configurePitDashboard(); } private void configureTelemetry() { @@ -325,6 +328,14 @@ private void configureTestOperatorBindings() { new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint)); } + public void configurePitDashboard() { + + Shuffleboard.getTab("Pit") + .add("Toggle HasAlgae", new ToggleHasAlgaeCommand(algaeSubsystem)) + .withPosition(3, 2) + .withSize(1, 1); + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java new file mode 100644 index 00000000..4b28ef4e --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java @@ -0,0 +1,19 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeStates; + +public class ToggleHasAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public ToggleHasAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + } + + @Override + public void initialize() { + algaeSubsystem.setState( + algaeSubsystem.getState() == AlgaeStates.EMPTY ? AlgaeStates.HAS_ALGAE : AlgaeStates.EMPTY); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java deleted file mode 100644 index f27d52e2..00000000 --- a/src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.commands.robotState; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.subsystems.robotState.RobotStateSubsystem; - -public class GetAlgaeCommand extends InstantCommand { - RobotStateSubsystem robotState; - - public GetAlgaeCommand(RobotStateSubsystem robotState) { - this.robotState = robotState; - } - - @Override - public void initialize() { - if (robotState.getGetAlgaeOnCycle() == true) { - robotState.setGetAlgaeOnCycle(false); - } else { - robotState.setGetAlgaeOnCycle(true); - } - } -} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java index 44dd4410..cdc5a457 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java @@ -12,6 +12,6 @@ public ToggleAlgaeHeightCommand(RobotStateSubsystem robotState) { @Override public void initialize() { - robotState.toggleAlgaeHeight(null); + robotState.toggleAlgaeHeight(); } } diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 0fa714bf..6e01e958 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -31,6 +31,8 @@ public class BiscuitConstants { public static double kCloseEnough = 0.05; // This was a little out of wack. public static final Angle kMaxFwd = Rotations.of(100); public static final Angle kMaxRev = Rotations.of(-100); + public static final double kSafeToStowUpper = 40; + public static final double kSafeToStowLower = -5; // Setpoints // Idle @@ -39,11 +41,11 @@ public class BiscuitConstants { public static final Angle kPrestageSetpoint = kStowSetpoint; // Algae removal - public static final Angle kL2AlgaeSetpoint = Rotations.of(120.12604); - public static final Angle kL3AlgaeSetpoint = Rotations.of(120.12604); + public static final Angle kL2AlgaeSetpoint = Rotations.of(19.11865); + public static final Angle kL3AlgaeSetpoint = Rotations.of(19.11865); - public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(120.12604); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(120.12604); + public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(19.11865); + public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(19.11865); public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); @@ -55,14 +57,14 @@ public class BiscuitConstants { public static final Angle kL4CoralSetpoint = kStowSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(320.74); - public static final Angle kMicAlgaeSetpoint = Rotations.of(324.33); - public static final Angle kHpAlgaeSetpoint = Rotations.of(106.6608); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(51.04735); + public static final Angle kMicAlgaeSetpoint = Rotations.of(51.61872); + public static final Angle kHpAlgaeSetpoint = Rotations.of(16.97559); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(319.6908); - public static final Angle kBargeSetpoint = Rotations.of(77.5906); - public static final Angle kBargeBackwardsSetpoint = Rotations.of(-77.5906); + public static final Angle kProcessorSetpoint = Rotations.of(50.88037); + public static final Angle kBargeSetpoint = Rotations.of(12.3489); + public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // jogging public static final double kJogAmountUp = 10; diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 20542ac3..d4026dc0 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -10,6 +10,8 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; public class DriveConstants { @@ -21,9 +23,16 @@ public class DriveConstants { public static final double kRateLimitFwdStr = 3.5; public static final double kRateLimitYaw = 8.0; - public static final double kDriveGearRatio = 6.5; + public static final double kDriveMotorOutputGear = 22; + public static final double kDriveInputGear = 52; + public static final double kBevelInputGear = 15; + public static final double kBevelOutputGear = 45; + + public static final double kDriveGearRatio = + (kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear); + public static final double kWheelDiameterInches = 4.0; - public static final double kMaxSpeedMetersPerSecond = 12.0; + public static final double kMaxSpeedMetersPerSecond = 3.384; public static final double kSpeedStillThreshold = 0.1; // meters per second public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second public static final double kGyroDifferentThreshold = 5.0; // 5 degrees @@ -61,6 +70,9 @@ public static Translation2d[] getWheelLocationMeters() { public static final double kRecoverTemp = 1290; public static final double kNotifyTemp = 1295; + public static final Pose2d kResetOdomPose = + new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67)); + // public static TalonFXSConfiguration // getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration // // constructor sets encoder to Quad/CTRE_MagEncoder_Relative diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 424f7a57..fe9cf9e4 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -6,11 +6,16 @@ public class TagServoingConstants { // Cameras public static final int kLeftServoCam = 0; // Score RIGHT coral - public static final int kRightServoCam = 1; // Score LEFT coral + public static final int kRightServoCam = 2; // Score LEFT coral + + // Offsets + public static final double kLeftCamOffset = VisionConstants.kCam1Pose.getY(); + public static final double kRightCamOffset = VisionConstants.kCam2Pose.getY(); // Targets public static final double kHorizontalTarget = 800; - public static final double kAreaTarget = 800; + public static final double kLeftCamDiagTarget = 1180; + public static final double kRightCamDiagTarget = 985; public static final double[] kAngleTarget = { Units.degreesToRadians(0), @@ -26,19 +31,21 @@ public class TagServoingConstants { public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6}; // Tag align - public static final double kAreaCloseEnough = 0; - public static final double kHorizontalCloseEnough = 0; + public static final double kHorizontalCloseEnough = 20; public static final double kAngleCloseEnough = Units.degreesToRadians(3.0); + public static final double kDiagCloseEnough = 20; + public static final double kNoUpdateMicrosec = 500_000; // Drive public static final double kInitialDriveRadius = 1.5; - public static final double kStopXDriveRadius = 1.2; // Should be closer to reef than target pose - public static final double kDriveCloseEnough = 0.3; + public static final double kStopXDriveRadius = + kInitialDriveRadius; // Should be closer to reef than target pose + public static final double kMinStopXDriveRadius = 1.35; + public static final double kDriveCloseEnough = 0.1; + public static final double kMinVelX = 0.85; // Reef - public static final Translation2d kBlueReefPose = - new Translation2d(Units.inchesToMeters(223.5), Units.inchesToMeters(158.5)); + public static final Translation2d kBlueReefPose = new Translation2d(4.524, 4.033); - public static final Translation2d kRedReefPose = - kBlueReefPose.plus(new Translation2d(Units.inchesToMeters(337.39), 0)); + public static final Translation2d kRedReefPose = new Translation2d(13.084, 4.033); } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index cbb8c7e3..1e6a8065 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -19,6 +19,7 @@ public final class VisionConstants { public static final double kMaxAmbig = 1.0; public static final int kMaxTimesOffWheels = 5; public static final double kBumperPixelLine = 87; // 100 + public static final double kRobotHeight = 0.5; // public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02); // public static final double kThetaStdDevRejected = Units.degreesToRadians(360); @@ -29,7 +30,7 @@ public final class VisionConstants { public static final double kOffsetOnVelFilter = 0.10; public static final double kSquaredCoeffOnVelFilter = 0.1; - public static Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0)); + public static Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0); public static final double kTimeStampOffset = 0.0; @@ -37,63 +38,85 @@ public final class VisionConstants { public static final double singleTagCoeff = 25.0 / 100.0; public static final double multiTagCoeff = 18.0 / 100.0; public static final double baseNumber = Math.E; - public static final double powerNumber = 4.0; + public static final double powerNumber = 2.0; + public static final double baseTrust = 3.0; + public static final double FOV45MultiTagCoeff = 16.0 / 100.0; public static final double FOV45powerNumber = 4.5; - public static final double FOV45SinlgeTagCoeff = 21.0 / 100.0; + public static final double FOV45SingleTagCoeff = 21.0 / 100.0; + public static final double FOV45BaseTrust = 2.0; + public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0; public static final double FOV58MJPGPowerNumber = 3.5; public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0; + public static final double FOV58MJPGBaseTrust = 3.0; + public static final double FOV58YUYVMultiTagCoeff = 17.0 / 100.0; - public static final double FOV58YUYVPowerNumber = 4.0; + public static final double FOV58YUYVPowerNumber = 2.0; public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV58YUYVBaseTrust = 3.0; + + public static final double FOV75YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV75YUYVPowerNumber = 2.0; + public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV75YUYVBaseTrust = 3.0; + + // Gyro error scaling + public static final double kYawErrorThreshold = Units.degreesToRadians(45); // Constants for cameras - public static final int kNumCams = 4; + public static final int kNumCams = 5; + public static final int kNumPis = 3; + public static final int[] kUdpIndex = {0, 1, 2}; + + // Camera Ports + public static final int[] kCamPorts = {5802, 5802, 5803, 5803, 5804}; // Names - public static final String kCam1Name = "Shooter"; - public static final String kCam2Name = "Intake"; - public static final String kCam3Name = "AngledShooterLeft"; - public static final String kCam4Name = "AngledShooterRight"; + public static final String kCam1Name = "Left Servo"; + public static final String kCam2Name = "Left High"; + public static final String kCam3Name = "Right Servo"; + public static final String kCam4Name = "Right High"; + public static final String kCam5Name = "Rear"; - public static final String kPi1Name = "Shooter"; - public static final String kPi2Name = "Intake"; - public static final String kPi3Name = "AngledShooters"; + public static final String kPi1Name = "Left"; + public static final String kPi2Name = "Right"; + public static final String kPi3Name = "Rear"; // Indexs public static final int kCam1Idx = 0; - public static final int kCam2Idx = 0; + public static final int kCam2Idx = 1; public static final int kCam3Idx = 0; public static final int kCam4Idx = 1; + public static final int kCam5Idx = 0; public static final double kLoopTime = 0.02; public static final int kCircularBufferSize = 1000; // Poses public static final Pose3d kCam1Pose = - new Pose3d( - new Translation3d(-0.27, 0.055, 0.20), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(180.0))); + new Pose3d(new Translation3d(0.28, 0.02, 0.30), new Rotation3d()); + public static final Pose3d kCam2Pose = new Pose3d( new Translation3d(-0.21, -0.31, 0.44), new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0))); public static final Pose3d kCam3Pose = - new Pose3d( - new Translation3d(-0.23, 0.33, 0.56), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(-132.0))); + new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d()); public static final Pose3d kCam4Pose = new Pose3d( new Translation3d(-0.22, -0.335, 0.50), new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); - + public static final Pose3d kCam5Pose = + new Pose3d( + new Translation3d(-0.22, -0.335, 0.50), + new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is // in the form [theta], with units in radians. public static Matrix kLocalMeasurementStdDevs = VecBuilder.fill(Units.degreesToRadians(0.01)); // Increase these numbers to trust global measurements from vision less. This matrix is in the - // form [x, y, theta], with units in meters and radians. + // form [x, y, theta]áµ€, with units in meters and radians. // Vision Odometry Standard devs public static Matrix kVisionMeasurementStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)); diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java index ed06e32e..18da2276 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -20,7 +20,7 @@ public class AlgaeSubsystem extends MeasurableSubsystem implements ClosedLoopSpe private final AlgaeIOInputs inputs = new AlgaeIOInputs(); private AngularVelocity desiredSpeed = RotationsPerSecond.of(0); - private AlgaeStates curState = AlgaeStates.IDLE; + private AlgaeStates curState = AlgaeStates.EMPTY; public AlgaeSubsystem(AlgaeIO io) { this.io = io; @@ -84,16 +84,16 @@ public void periodic() { switch (curState) { case HAS_ALGAE -> { - if (!inputs.isLimitSwitchClosed) { - setState(AlgaeStates.EMPTY); - setSpeed(RotationsPerSecond.of(0)); - } + // if (!inputs.isLimitSwitchClosed) { + // setState(AlgaeStates.EMPTY); + // setSpeed(RotationsPerSecond.of(0)); + // } } case EMPTY -> { - if (inputs.isLimitSwitchClosed) { // FIXME: correct? - hold(); - setState(AlgaeStates.HAS_ALGAE); - } + // if (inputs.isLimitSwitchClosed) { // FIXME: correct? + // hold(); + // setState(AlgaeStates.HAS_ALGAE); + // } } case IDLE -> {} } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java index 02d8743e..18c38d3b 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java @@ -1,10 +1,6 @@ package frc.robot.subsystems.biscuit; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -12,8 +8,8 @@ public interface BiscuitIO { @AutoLog public class BiscuitIOInputs { - public Angle position = Rotations.of(0); - public AngularVelocity velocity = RotationsPerSecond.of(0); + public double position = 0.0; + public double velocity = 0.0; public boolean fwdLimitSwitchOpen = false; public boolean didZero; } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index a38b5475..dc6ff6be 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -68,8 +68,8 @@ public void setPosition(Angle position) { @Override public void updateInputs(BiscuitIOInputs inputs) { - inputs.velocity = velocity.getValue(); - inputs.position = position.getValue(); + inputs.velocity = velocity.getValueAsDouble(); + inputs.position = position.getValueAsDouble(); inputs.didZero = didZero; BaseStatusSignal.refreshAll(velocity, position); } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java index cb98dba4..0c927b5a 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java @@ -3,6 +3,7 @@ package frc.robot.subsystems.biscuit; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -32,11 +33,11 @@ public void setPosition(Angle position) { @Override public Angle getPosition() { - return inputs.position; + return Rotations.of(inputs.position); } public AngularVelocity getVelocity(AngularVelocity velocity) { - return inputs.velocity; + return RotationsPerSecond.of(inputs.velocity); } @Override @@ -46,14 +47,19 @@ public void zero() { @Override public boolean isFinished() { - return setPoint.minus(inputs.position).abs(Rotations) < BiscuitConstants.kCloseEnough; + return Math.abs(getPosition().minus(setPoint).in(Rotations)) < BiscuitConstants.kCloseEnough; + } + + public boolean isSafeToStow() { + return getPosition().in(Rotations) < BiscuitConstants.kSafeToStowUpper + && getPosition().in(Rotations) > BiscuitConstants.kSafeToStowLower; } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs(getName(), inputs); - Logger.recordOutput("Biscuit setPoint", setPoint); + Logger.recordOutput("Biscuit setPoint", setPoint.in(Rotations)); Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index df9c8f45..7128d394 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -2,6 +2,7 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -9,8 +10,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.robotState.RobotStateSubsystem; import java.util.Set; @@ -166,6 +170,29 @@ public Trajectory getAutoTrajectory() { } } + public void addVisionMeasurement(Pose2d pose, double timestamp) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000 - timestamp); + io.addVisionMeasurement(pose, timestamp); + } + + public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevvs) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/stdDevvs", stdDevvs.get(0, 0)); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000.0 - timestamp); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/FPGA Seconds", RobotController.getFPGATime() / 1_000_000.0); + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Result Seconds", timestamp); + + io.addVisionMeasurement(pose, timestamp, stdDevvs); + } + public void resetHolonomicController(double yaw) { xController.reset(); yController.reset(); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 5c095048..54176f4b 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -48,9 +48,9 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private ScoringLevel scoringLevel = ScoringLevel.L4; private ScoringLevel currentLevel = ScoringLevel.L4; - private ScoreSide scoreSide; - private AlgaeHeight algaeHeight; - private CoralLoc coralLoc; + private ScoreSide scoreSide = ScoreSide.LEFT; + private AlgaeHeight algaeHeight = AlgaeHeight.LOW; + private CoralLoc coralLoc = CoralLoc.CORAL; private boolean isAutoPlacing = false; private boolean getAlgaeOnCycle = false; @@ -108,7 +108,7 @@ public Alliance getAllianceColor() { } public ScoringLevel getAlgaeLevel() { - return tagAlignSubsystem.computeHexant(allianceColor) % 2 == 0 + return (tagAlignSubsystem.computeHexant(allianceColor) % 2) == 0 ? ScoringLevel.L3 : ScoringLevel.L2; } @@ -133,8 +133,8 @@ private void setState(RobotStates robotState, boolean transfer) { curState = RobotStates.TRANSFER; } else { logger.info("{} -> {}", this.curState, robotState); + curState = nextState = robotState; } - curState = nextState = robotState; } } @@ -154,8 +154,8 @@ public void setAlgaeHeight(AlgaeHeight algaeHeight) { this.algaeHeight = algaeHeight; } - public void toggleAlgaeHeight(AlgaeHeight algaeHeight) { - this.algaeHeight = algaeHeight == AlgaeHeight.LOW ? AlgaeHeight.HIGH : AlgaeHeight.LOW; + public void toggleAlgaeHeight() { + algaeHeight = algaeHeight == AlgaeHeight.LOW ? AlgaeHeight.HIGH : AlgaeHeight.LOW; } public void setAutoPlacing(boolean isAutoPlacing) { @@ -245,6 +245,14 @@ public void toStow() { setState(RobotStates.TO_STOW); } + public void toStowSafe() { + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); + // algaeSubsystem.hold(); + + setState(RobotStates.TO_STOW_SAFE); + } + public void toPrepCoral() { if (curState == RobotStates.REEF_ALIGN_CORAL) { if (elevatorSubsystem.isFinished()) { @@ -384,7 +392,7 @@ public void toScoreAlgae() { if (poseX > RobotStateConstants.kRedBargeSafeX || poseX < RobotStateConstants.kBlueBargeSafeX) { - biscuitSubsystem.setPosition(BiscuitConstants.kBargeSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kBargeBackwardSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); setState(RobotStates.BARGE_ALGAE, true); @@ -469,6 +477,8 @@ public void periodic() { Logger.recordOutput("RobotState/state", curState); Logger.recordOutput("RobotState/hasCoral", hasCoral()); Logger.recordOutput("RobotState/scoringLevel", scoringLevel); + Logger.recordOutput("RobotState/algeaHeight", algaeHeight); + Logger.recordOutput("RobotState/getAlgea", getAlgaeOnCycle); switch (curState) { case TRANSFER -> { @@ -484,6 +494,14 @@ public void periodic() { setState(RobotStates.STOW); } } + + case TO_STOW_SAFE -> { + if (biscuitSubsystem.isSafeToStow()) { + elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + setState(RobotStates.TO_STOW); + } + } + case STOW -> { if (futureState != null) { switch (futureState) { @@ -520,13 +538,13 @@ public void periodic() { biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint); - // FIXME: if driving to remove algae - switch (scoringLevel) { - case L1, L2 -> { - driveSubsystem.move(DriveConstants.kAlgaeRemovalSpeed, 0, 0, false); - } - default -> {} - } + // // FIXME: if driving to remove algae + // switch (scoringLevel) { + // case L1, L2 -> { + // driveSubsystem.move(DriveConstants.kAlgaeRemovalSpeed, 0, 0, false); + // } + // default -> {} + // } } case L3 -> { biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint); @@ -552,13 +570,14 @@ public void periodic() { if (getAlgaeLevel() == ScoringLevel.L2) { switch (scoringLevel) { case L1, L2 -> { - // FIXME: if driving to remove algae - if (driveSubsystem.getPoseMeters().minus(algaeRemovalPose).getTranslation().getNorm() - > RobotStateConstants.kAlgaeRetreatDistance) { - driveSubsystem.move(0, 0, 0, false); - toReefAlign(false, true); - } - // FIXME: if not driving to remove algae + // // FIXME: if driving to remove algae + // if + // (driveSubsystem.getPoseMeters().minus(algaeRemovalPose).getTranslation().getNorm() + // > RobotStateConstants.kAlgaeRetreatDistance) { + // driveSubsystem.move(0, 0, 0, false); + // toReefAlign(false, true); + // } + // // FIXME: if not driving to remove algae biscuitSubsystem.setPosition(BiscuitConstants.kSafeAlgaeRemovalSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kSafeAlgaeRemovalSetpoint); setState(RobotStates.SAFE_REMOVE_ALGAE_ABOVE, true); @@ -619,22 +638,22 @@ public void periodic() { } case PROCESSOR_ALGAE -> { if (!algaeSubsystem.hasAlgae()) { - toStow(); + toStowSafe(); } } case BARGE_ALGAE -> { if (!algaeSubsystem.hasAlgae()) { - toStow(); + toStowSafe(); } } case MIC_ALGAE -> { if (algaeSubsystem.hasAlgae()) { - toStow(); + toStowSafe(); } } case FLOOR_ALGAE -> { if (algaeSubsystem.hasAlgae()) { - toStow(); + toStowSafe(); } } case PREP_CLIMB -> {} @@ -658,6 +677,7 @@ public void registerWith(TelemetryService telemetryService) { public enum RobotStates { STOW, TO_STOW, + TO_STOW_SAFE, PREP_CORAL, REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 703e2a9f..13ab380e 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -5,10 +5,12 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -37,26 +39,31 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private int targetCamId; private int targetTagId; private int fieldRelHexant; - private Alliance alliance; + private Alliance alliance = Alliance.Blue; + private double goalTargetDiag; + + private long startServoTime; public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) { this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; // FIXME: need sane constants - this.driveX = new ProfiledPIDController(0.0019, 0, 0, new Constraints(3.0, 3.0)); - this.driveY = new ProfiledPIDController(0.00001, 0, 0, new Constraints(2.0, 1.0)); + this.driveX = new ProfiledPIDController(3, 0, 0, new Constraints(1, 1.0)); + this.driveY = new ProfiledPIDController(5, 0, 0, new Constraints(2, 3)); this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0)); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - this.alignX = new ProfiledPIDController(0.0019, 0, 0, new Constraints(3.0, 3.0)); - this.alignY = new ProfiledPIDController(0.00001, 0, 0, new Constraints(2.0, 1.0)); - this.alignOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0)); + this.alignX = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); // 0.0015 + this.alignY = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); + this.alignOmega = new ProfiledPIDController(5.5, 0, 0, new Constraints(1.0, 1.0)); this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - Logger.recordOutput("TagAlignSubsystem/TargetArea", -1); + Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1); Logger.recordOutput("TagAlignSubsystem/TargetTag", -1); Logger.recordOutput("TagAlignSubsystem/TargetCenterX", -1); + Logger.recordOutput("TagAlignSubsystem/Hexant", -1); + Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", -1); } public int computeHexant(Alliance color) { @@ -66,19 +73,23 @@ public int computeHexant(Alliance color) { : TagServoingConstants.kRedReefPose; double offset = Units.degreesToRadians(30); - return (((int) - (FastMath.normalizeZeroTwoPi( + int hexant = + (((int) + (FastMath.normalizeZeroTwoPi( driveSubsystem .getPoseMeters() .getTranslation() .minus(reefT) .getAngle() .getRadians() - - offset) - / Units.degreesToRadians(60) - + offset)) - + (color == Alliance.Blue ? 0 : 3)) - % 6; + + offset) + / Units.degreesToRadians(60))) + + (color == Alliance.Blue ? 3 : 0)) + % 6; + + Logger.recordOutput("TagAlignSubsystem/Hexant", hexant); + + return hexant; } // Red reef numbered like blue (red 0 is facing the same direction as blue 0) @@ -86,7 +97,7 @@ private int computeFieldRelHexant(Alliance color) { return (computeHexant(color) + (color == Alliance.Blue ? 0 : 3)) % 6; } - private Pose2d getTargetDrivePose(Alliance color) { + private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) { Translation2d reefT = color == Alliance.Blue ? TagServoingConstants.kBlueReefPose @@ -95,10 +106,16 @@ private Pose2d getTargetDrivePose(Alliance color) { Translation2d offset = new Translation2d( TagServoingConstants.kInitialDriveRadius, - Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); + Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180)); + + Translation2d sideOffset = + new Translation2d( + scoreLeft ? TagServoingConstants.kRightCamOffset : TagServoingConstants.kLeftCamOffset, + Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180 + 90)); return new Pose2d( - reefT.plus(offset), Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); + reefT.plus(offset).plus(sideOffset), + Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); } private double getCurRadius(Alliance color) { @@ -106,7 +123,9 @@ private double getCurRadius(Alliance color) { color == Alliance.Blue ? TagServoingConstants.kBlueReefPose : TagServoingConstants.kRedReefPose; - return driveSubsystem.getPoseMeters().getTranslation().minus(reefT).getNorm(); + + Translation2d reefRelative = driveSubsystem.getPoseMeters().getTranslation().minus(reefT); + return FastMath.hypot(reefRelative.getX(), reefRelative.getY()); } public TagAlignStates getState() { @@ -114,8 +133,12 @@ public TagAlignStates getState() { } public void setup(Alliance alliance, boolean scoreLeft) { - targetPose = getTargetDrivePose(alliance); + targetPose = getTargetDrivePose(alliance, scoreLeft); this.alliance = alliance; + this.goalTargetDiag = + scoreLeft + ? TagServoingConstants.kRightCamDiagTarget + : TagServoingConstants.kLeftCamDiagTarget; // Inverted, scoring left coral means aligning right camera targetCamId = @@ -128,6 +151,8 @@ public void setup(Alliance alliance, boolean scoreLeft) { fieldRelHexant = computeFieldRelHexant(alliance); Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); + Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); + Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); Pose2d current = driveSubsystem.getPoseMeters(); @@ -135,8 +160,8 @@ public void setup(Alliance alliance, boolean scoreLeft) { driveY.reset(current.getY()); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - alignX.reset(current.getX()); - alignY.reset(current.getY()); + alignX.reset(0); + alignY.reset(0); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -144,6 +169,10 @@ public void setup(Alliance alliance, boolean scoreLeft) { public double calculateAlignY() { WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); + if (result == null) { + return 2767; + } + int tagIndex = -1; int[] tags = result.getTagIDs(); @@ -160,10 +189,12 @@ public double calculateAlignY() { } Point center = result.getTagCenters().get(tagIndex); + double diag = result.getTagDiags()[tagIndex]; + Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); - double vY = alignY.calculate(center.x(), TagServoingConstants.kHorizontalTarget); + double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); return vY; } @@ -176,12 +207,14 @@ public void start(Alliance alliance, boolean scoreLeft) { public void terminate() { driveSubsystem.move(0, 0, 0, false); + driveSubsystem.drive(0, 0, 0); curState = TagAlignStates.DONE; } @Override public void periodic() { Logger.recordOutput("TagAlignSubsystem/State", curState.toString()); + // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance)); switch (curState) { case DRIVE -> { @@ -190,9 +223,13 @@ public void periodic() { double vX = driveX.calculate(current.getX(), targetPose.getX()); double vY = driveY.calculate(current.getY(), targetPose.getY()); double vOmega = - driveX.calculate( + driveOmega.calculate( current.getRotation().getRadians(), targetPose.getRotation().getRadians()); + Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); + Translation2d tagRelVel = new Translation2d(vX, vY) .rotateBy( @@ -201,21 +238,43 @@ public void periodic() { double tagRelX = tagRelVel.getX(); double tagRelY = tagRelVel.getY(); - if (getCurRadius(alliance) < TagServoingConstants.kStopXDriveRadius) { + double radius = getCurRadius(alliance); + boolean ignoreX = radius < TagServoingConstants.kStopXDriveRadius; + + tagRelX = tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; + + if (radius < TagServoingConstants.kStopXDriveRadius) { tagRelX = 0; } - if (Math.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough - && targetPose.minus(current).getTranslation().getNorm() - < TagServoingConstants.kDriveCloseEnough) { - alignX.reset(current.getX()); - alignY.reset(current.getY()); + Translation2d tagRelError = + targetPose + .minus(current) + .getTranslation() + .rotateBy( + Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + + Logger.recordOutput("TagAlignSubsystem/Tag Rel Y Error", tagRelError.getY()); + + Transform2d poseError = targetPose.minus(current); + + if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough + && (FastMath.hypot(poseError.getX(), poseError.getY()) + < TagServoingConstants.kDriveCloseEnough + || ignoreX + && FastMath.abs(tagRelError.getY()) < TagServoingConstants.kDriveCloseEnough)) { + alignX.reset(0); + alignY.reset(0); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; + startServoTime = RobotController.getFPGATime(); break; } + Logger.recordOutput("TagAlignSubsystem/Tag Rel vX", tagRelX); + Logger.recordOutput("TagAlignSubsystem/Tag Rel vY", tagRelY); + Translation2d adjusted = new Translation2d(tagRelX, tagRelY) .rotateBy( @@ -227,6 +286,10 @@ public void periodic() { case TAG_ALIGN -> { WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); + if (result == null) { + return; + } + int tagIndex = -1; int[] tags = result.getTagIDs(); @@ -242,19 +305,33 @@ public void periodic() { } Point center = result.getTagCenters().get(tagIndex); - double area = result.getTagAreas()[tagIndex]; + double diag = result.getTagDiags()[tagIndex]; - Logger.recordOutput("TagAlignSubsystem/TargetArea", area); + Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); - double vX = alignX.calculate(area, TagServoingConstants.kAreaTarget); - double vY = alignY.calculate(center.x(), TagServoingConstants.kHorizontalTarget); + double vX = -alignX.calculate(goalTargetDiag - diag, 0); + double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); + + Logger.recordOutput("TagAlignSubsystem/X Error", alignX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/Y Error", alignY.getPositionError()); + Logger.recordOutput( + "TagAlignSubsystem/Last result delay", + (RobotController.getFPGATime() - result.getTimeStamp()) / 1000); + + if (RobotController.getFPGATime() - result.getTimeStamp() + > TagServoingConstants.kNoUpdateMicrosec + && RobotController.getFPGATime() - startServoTime + > TagServoingConstants.kNoUpdateMicrosec) { + terminate(); + break; + } - if (TagServoingConstants.kAreaTarget - area < TagServoingConstants.kAngleCloseEnough - || area > TagServoingConstants.kAreaTarget) { + if (FastMath.abs(goalTargetDiag - diag) < TagServoingConstants.kDiagCloseEnough + || diag > goalTargetDiag) { vX = 0; - if (Math.abs(TagServoingConstants.kHorizontalTarget - center.x()) + if (FastMath.abs(TagServoingConstants.kHorizontalTarget - center.x()) < TagServoingConstants.kHorizontalCloseEnough) { terminate(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 48e2e5a7..bc423cd9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,13 +1,478 @@ package frc.robot.subsystems.vision; -import WallEye.WallEyeTagResult; +import static edu.wpi.first.units.Units.*; -public class VisionSubsystem { - public VisionSubsystem() {} +import WallEye.*; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.drive.DriveSubsystem; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Set; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class VisionSubsystem extends MeasurableSubsystem { + + // Array of cameras + private WallEyeCam[] cams; + + // Array of camera positions + private Translation3d[] camPositions = { + VisionConstants.kCam1Pose.getTranslation(), + VisionConstants.kCam2Pose.getTranslation(), + VisionConstants.kCam3Pose.getTranslation(), + VisionConstants.kCam4Pose.getTranslation(), + VisionConstants.kCam5Pose.getTranslation() + }; + + // Array of camera rotations + private Rotation3d[] camRotations = { + VisionConstants.kCam1Pose.getRotation(), + VisionConstants.kCam2Pose.getRotation(), + VisionConstants.kCam3Pose.getRotation(), + VisionConstants.kCam4Pose.getRotation(), + VisionConstants.kCam5Pose.getRotation() + }; + + // Array of camera names + private String[] camNames = { + VisionConstants.kCam1Name, + VisionConstants.kCam2Name, + VisionConstants.kCam3Name, + VisionConstants.kCam4Name, + VisionConstants.kCam5Name + }; + + // Array of orange pi names + private String[] piNames = { + VisionConstants.kPi1Name, + VisionConstants.kPi1Name, + VisionConstants.kPi2Name, + VisionConstants.kPi2Name, + VisionConstants.kPi3Name + }; + + // Array of camera indexs + private int[] camIndex = { + VisionConstants.kCam1Idx, + VisionConstants.kCam2Idx, + VisionConstants.kCam3Idx, + VisionConstants.kCam4Idx, + VisionConstants.kCam5Idx + }; + + private DriveSubsystem driveSubsystem; + /*Because we use two seperate loggers we can import one and then define the + other here.*/ + private org.slf4j.Logger textLogger; + private UdpSubscriber[] udpSubscriber; + private AprilTagFieldLayout field; + private boolean visionUpdating = true; + private int minTags; + private CircularBuffer gyroBuffer = + new CircularBuffer(VisionConstants.kCircularBufferSize); + private double timeSinceLastUpdate; + private int updatesToWheels; + private Matrix adaptiveMatrix; + private ArrayList> validResults = new ArrayList<>(); + private WallEyeTagResult[] lastResult = new WallEyeTagResult[VisionConstants.kNumCams]; + private Matrix adativeMatrix; + private Matrix stdMatrix; + + public VisionSubsystem(DriveSubsystem driveSubsystem) { + this.driveSubsystem = driveSubsystem; + textLogger = LoggerFactory.getLogger("Vision"); + + cams = new WallEyeCam[VisionConstants.kNumCams]; + udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis]; + // We copy the matrix from our constants + adaptiveMatrix = VisionConstants.kVisionMeasurementStdDevs.copy(); + // We then copy the adaptive matrix because they will differ later + stdMatrix = adaptiveMatrix.copy(); + // I'm not sure why we put this in a try catch maybe could be removed + try { + field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile); + } catch (IOException e) { + textLogger.error("BAD NEWS BEARS NO APRIL TAG LAYOUT"); + } + // Fill our camera array + for (int i = 0; i < VisionConstants.kNumCams; i++) { + cams[i] = new WallEyeCam(piNames[i], camIndex[i], -1); + } + // Initialize our udpSubscribers + udpSubscriber[0] = new UdpSubscriber(5802, cams[0], cams[1]); + udpSubscriber[1] = new UdpSubscriber(5803, cams[2], cams[3]); + udpSubscriber[2] = new UdpSubscriber(5804, cams[4]); + } + // Setter Methods + public void setVisionUpdating(boolean updating) { + this.visionUpdating = updating; + } + + public void setMinTags(int minTags) { + this.minTags = minTags; + } + // Getter Methods + public boolean isVisionUpdating() { + return visionUpdating; + } + + public boolean cameraConnected(int index) { + return cams[index].isCameraConnected(); + } + + private double getSeconds() { + return RobotController.getFPGATime() / 1_000_000; + } + + private double minTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + int[] ids = result.getTagIDs(); + // This number's value doesn't really matter it just needs to be large + double minDistance = 2000; + + // Go through the camera locations and finds the one closest to the tag + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + + double dist = FastMath.hypot(tagPose.getX() - camLoc.getX(), tagPose.getY() - camLoc.getY()); + + if (dist < minDistance) { + minDistance = dist; + } + } + return minDistance; + } + + private double averageTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + + int[] ids = result.getTagIDs(); + double totalDistance = 0.0; + + // Goes through the camera locations and gets the average distance + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + totalDistance += + FastMath.hypot(tagPose.getX() - camLoc.getX(), tagPose.getY() - camLoc.getY()); + } + return totalDistance / ids.length; + } + + // Filters + private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) { + + ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed(); + Pose2d curPose = driveSubsystem.getPoseMeters(); + + Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d())); + + double velMagnitude = + FastMath.sqrt( + FastMath.pow(vel.vxMetersPerSecond, 2) + FastMath.pow(vel.vyMetersPerSecond, 2)); + + double dispMagnitude = + FastMath.sqrt(FastMath.pow(disp.getX(), 2) + FastMath.pow(disp.getY(), 2)); + + /*This gets our displacement and compares it to who much we could + have moved.It does this by getting the velocity and plotting it on a + graph. The graph will be in the docs.*/ + return result.getNumTags() >= minTags + && dispMagnitude + <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter + + VisionConstants.kOffsetOnVelFilter + + FastMath.pow(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter, 2)); + } + + private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) { + return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig) + && pose.getX() < field.getFieldLength() + && pose.getX() > 0 + && pose.getY() < field.getFieldWidth() + && pose.getY() > 0; + } + + /*Large switch case to see get the standard deviation factor based on camera, how many + tags we see and distance. An example of this graph will be in the docs.*/ + private double getStdDevFactor(double distance, int numTags, String camName) { + switch (camName) { + case VisionConstants.kCam2Name, VisionConstants.kCam4Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58YUYVSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58YUYVMultiTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + case VisionConstants.kCam5Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + + case VisionConstants.kCam1Name, VisionConstants.kCam3Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV75YUYVSingleTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV75YUYVMultiTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + } + + default -> { + if (numTags == 1) + return 1 + / (VisionConstants.baseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.singleTagCoeff * distance, VisionConstants.powerNumber))); + return 1 + / (VisionConstants.baseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.multiTagCoeff * distance, VisionConstants.powerNumber))); + } + } + } + + private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) { + /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro + from the rotation of the pose and compare the two*/ + double pose1Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose1.getRotation().toRotation2d()) + .getRadians()); + double pose2Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose2.getRotation().toRotation2d()) + .getRadians()); + + Logger.recordOutput("Vision/Pose 1 Yaw Error", pose1Error); + Logger.recordOutput("Vision/Pose 2 Yaw Error", pose2Error); + Logger.recordOutput("Vision/Historical yaw", rotation); + + if (pose1Error < pose2Error) { + if (pose1Error > VisionConstants.kYawErrorThreshold) {} + + return pose1; + } else { + return pose2; + } + } + + private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) { + // double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ()); + // double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ()); + // // This filters out results by seeing if they are close to the right height + // if (dist1 < dist2 && dist1 < 0.5) { + // return pose1; + // } + // if (dist2 < dist1 && dist2 < 0.5) { + // return pose2; + // } + // If we don't have enough data in the gyro buffer we default to returning a pose + if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1; + + // See what pose is closer the the gyro at the time of the photo's capture. + double rotation = + gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation); + } + + @Override + public void periodic() { + double gyroData = FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians()); + gyroBuffer.addFirst(gyroData); + + if (gyroBuffer.size() >= 1000) { + Logger.recordOutput("Vision/Gyro Buffer Delayed", gyroBuffer.get(25)); + } + + Logger.recordOutput("Vision/Gyro Buffer", gyroData); + + if (getSeconds() - timeSinceLastUpdate > VisionConstants.kMaxTimeNoVision) { + updatesToWheels = 0; + } + + validResults.clear(); + + for (int i = 0; i < VisionConstants.kNumCams; i++) { + if (cams[i].hasNewUpdate()) { + timeSinceLastUpdate = getSeconds(); + validResults.add(new Pair(cams[i].getResults(), i)); + lastResult[i] = (WallEyeTagResult) cams[i].getResults(); + } + } + + if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) { + // Decrease the thresholds required for a good pose over time. A graph is in the docs + for (int i = 0; i < 2; i++) { + double scaledWeight = + VisionConstants.kVisionMeasurementStdDevs.get(i, 0) + + VisionConstants.kStdDevDecayCoeff + * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev); + + adaptiveMatrix.set( + i, + 0, + scaledWeight >= VisionConstants.kMinStdDev ? scaledWeight : VisionConstants.kMinStdDev); + } + } + + for (Pair res : validResults) { + if (res.getFirst() instanceof WallEyePoseResult) { + /*Reset the adaptive matrix to a stricter value once we get a result. + We set it to a stricter value then initially.*/ + adaptiveMatrix.set(0, 0, .1); + adaptiveMatrix.set(1, 0, .1); + + WallEyePoseResult result = (WallEyePoseResult) res.getFirst(); + + if (result.getCameraPose() == null) { // TODO figure out why it sometimes is null + textLogger.error("Pose is null, skipping!"); + continue; + } + + int idx = res.getSecond(); + + for (int i = 0; i < 2; i++) { + stdMatrix.set( + i, + 0, + .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx])); + } + + Pose3d cameraPose; + Translation3d robotTranslation = new Translation3d(); + Rotation3d cameraRotation = new Rotation3d(); + + if (result.getNumTags() > 1) { + // If there our more then one tag in an image we can get pose possible pose + cameraPose = result.getCameraPose(); + + robotTranslation = + cameraPose + .getTranslation() + .minus( + camPositions[idx] + .rotateBy(cameraPose.getRotation()) + .rotateBy(camRotations[idx])); + + cameraRotation = cameraPose.getRotation().rotateBy(camRotations[idx]); + } else { + // If there is one we get two possible poses and have to filter them. + Pose3d cam1Pose = result.getFirstPose(); + Pose3d cam2Pose = result.getSecondPose(); + + cam1Pose = + new Pose3d( + cam1Pose.getTranslation(), cam1Pose.getRotation().rotateBy(camRotations[idx])); + cam2Pose = + new Pose3d( + cam2Pose.getTranslation(), cam2Pose.getRotation().rotateBy(camRotations[idx])); + + Logger.recordOutput("Vision/Raw Camera 1 " + camNames[idx], cam1Pose); + Logger.recordOutput("Vision/Raw Camera 2 " + camNames[idx], cam2Pose); + + cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx); + robotTranslation = + cameraPose + .getTranslation() + .minus(camPositions[idx].rotateBy(cameraPose.getRotation())) + .rotateBy(camRotations[idx]); + cameraRotation = cameraPose.getRotation(); + } + Pose2d robotPose = + new Pose2d(robotTranslation.toTranslation2d(), cameraRotation.toRotation2d()); + if (camsWithinField(robotTranslation, result)) { + // Is the pose in the field? If so, enjoy a updated position drive subsystem + updatesToWheels++; + Logger.recordOutput("Vision/Accepted Cam " + camNames[idx], robotPose); + // However we do have to be accepting the poses to use them + if (visionUpdating) { + driveSubsystem.addVisionMeasurement( + robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); + } + } else { + Logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose); + } + } + } + } - // FIXME: see walleye-testing branch from crescendo for correct implementation but - // FIXME: USE WalleyeTagResult INSTEAD OF WalleyeResult! public WallEyeTagResult getLastResult(int index) { - return null; + return lastResult[index]; + } + + @Override + public Set getMeasures() { + return Set.of(); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + super.registerWith(telemetryService); } } From e1d66455a51c938ed849512ef42d7e2cdb05e372 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 18 Feb 2025 21:17:26 -0500 Subject: [PATCH 14/16] all the excelent changes --- .../robotState/ScoreAlgaeCommand.java | 9 +- .../frc/robot/constants/BiscuitConstants.java | 15 ++-- .../frc/robot/constants/DriveConstants.java | 1 + .../robot/constants/ElevatorConstants.java | 7 +- .../robot/constants/RobotStateConstants.java | 4 +- .../robot/subsystems/biscuit/BiscuitIOFX.java | 16 ++-- .../subsystems/biscuit/BiscuitSubsystem.java | 2 +- .../elevator/ElevatorSubsystem.java | 5 ++ .../robotState/RobotStateSubsystem.java | 82 ++++++++++++++++--- vendordeps/thirdcoast.json | 4 +- 10 files changed, 105 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java index cf919934..380863f7 100644 --- a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java @@ -27,7 +27,11 @@ public ScoreAlgaeCommand( public void initialize() { startState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); - robotStateSubsystem.toScoreAlgae(); + if (startState == RobotStates.BARGE_ALGAE || startState == RobotStates.PROCESSOR_ALGAE) { + robotStateSubsystem.releaseAlgae(); + } else { + robotStateSubsystem.toScoreAlgae(); + } } @Override @@ -38,7 +42,8 @@ public boolean isFinished() { || !startingElevatorFinished; } else { return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE - || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE; + || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE + || !robotStateSubsystem.isBargeSafe; } } } diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 6e01e958..a90d15db 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -23,14 +23,13 @@ import edu.wpi.first.units.measure.Angle; public class BiscuitConstants { - // These are all wrong right now because we don't have any actual info - public static double kZero = 0.02; // TODO Will need to be experimentally determined + public static double kZero = .37; public static double kTicksPerRot = 160; public static int talonID = 25; - public static double kCloseEnough = 0.05; // This was a little out of wack. - public static final Angle kMaxFwd = Rotations.of(100); - public static final Angle kMaxRev = Rotations.of(-100); + public static double kCloseEnough = 0.05; + public static final Angle kMaxFwd = Rotations.of(51.04735 + 5); + public static final Angle kMaxRev = Rotations.of(-12.3489 - 5); public static final double kSafeToStowUpper = 40; public static final double kSafeToStowLower = -5; @@ -70,7 +69,7 @@ public class BiscuitConstants { public static final double kJogAmountUp = 10; public static final double kJogAmountDown = -10; - // Disables the TalonFXS by setting it's voltage to zero. Not very shocking. + // Disables the TalonFXS by setting its voltage to zero. public static VoltageConfigs disableTalon() { VoltageConfigs voltage = new VoltageConfigs().withPeakForwardVoltage(0.0).withPeakReverseVoltage(0.0); @@ -105,9 +104,9 @@ public static TalonFXSConfiguration getFXSConfig() { SoftwareLimitSwitchConfigs swLimit = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(false) + .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(kMaxFwd) - .withReverseSoftLimitEnable(false) + .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(kMaxRev); fxsConfig.SoftwareLimitSwitch = swLimit; diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index d4026dc0..9c54826b 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -41,6 +41,7 @@ public class DriveConstants { public static final double kRobotLength = 0.6223; public static final double kRobotWidth = 0.6223; public static final double kFieldMaxX = 17.526; + public static final double kCenterLineX = 8.763; public static final double kPOmega = 4.5; public static final double kIOmega = 0.0; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 53ec4664..27da7482 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -44,13 +44,13 @@ public class ElevatorConstants { // Algae removal public static final Angle kL2AlgaeSetpoint = Rotations.of(2.8677); - public static final Angle kL3AlgaeSetpoint = Rotations.of(2.8677); + public static final Angle kL3AlgaeSetpoint = Rotations.of(14.724); public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(2.8677); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(2.8677); + public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(14.724); public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(2.8677); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(2.8677); + public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(14.724); // Coral score public static final Angle kL1CoralSetpoint = Rotations.of(13.04053); @@ -69,6 +69,7 @@ public class ElevatorConstants { // Algae scoring public static final Angle kProcessorSetpoint = Rotations.of(14.9063); public static final Angle kBargeSetpoint = Rotations.of(40.913); + public static final Angle kBargeHigherThan = Rotations.of(31.0901); public static TalonFXConfiguration getBothFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index 06df4d69..1ddc8768 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -4,8 +4,8 @@ public class RobotStateConstants { public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0}; public static final double kAlgaeRetreatDistance = 0; - public static final double kRedBargeSafeX = 9.5; - public static final double kBlueBargeSafeX = 8; + public static final double kBlueBargeSafeX = 7.6; + public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX; public static final double kCoralEjectTimer = 0.5; } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index dc6ff6be..c8acc2dd 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -82,16 +82,10 @@ public void registerWith(TelemetryService telemetry) { @Override public void zero() { didZero = false; - if (fwdLimitSwitchOpen == true) { - double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1); - double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos); - talon.setPosition(setPos); - logger.info("set Biscuit position to " + setPos); - didZero = true; - } else { - rangeAlert.set(true); - logger.error("Biscuit overextended! Shutting down movement!"); - configurator.apply(BiscuitConstants.disableTalon()); - } + double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1); + double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos); + talon.setPosition(setPos); + logger.info("set Biscuit position to " + setPos); + didZero = true; } } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java index 0c927b5a..2ef3ea40 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java @@ -60,7 +60,7 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs(getName(), inputs); Logger.recordOutput("Biscuit setPoint", setPoint.in(Rotations)); - Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0); + Logger.recordOutput("Is Biscuit Finished", isFinished()); } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 0e924d06..f09ed333 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -48,6 +48,11 @@ public boolean isFinished() { && currState != ElevatorStates.ZEROING; } + public boolean isHigherThan(Angle higherThanPos) { + return getPosition().in(Rotations) > higherThanPos.in(Rotations) + && currState != ElevatorStates.ZEROING; + } + @Override public void zero() { currState = ElevatorStates.ZEROING; diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 54176f4b..abde5a6a 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.robotState; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.BiscuitConstants; @@ -50,6 +51,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private ScoringLevel currentLevel = ScoringLevel.L4; private ScoreSide scoreSide = ScoreSide.LEFT; private AlgaeHeight algaeHeight = AlgaeHeight.LOW; + private AlgaeHeight currentAlgaeHeight = AlgaeHeight.LOW; private CoralLoc coralLoc = CoralLoc.CORAL; private boolean isAutoPlacing = false; @@ -60,6 +62,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private Timer scoringTimer = new Timer(); private Pose2d algaeRemovalPose; + public boolean isBargeSafe = true; public RobotStateSubsystem( AlgaeSubsystem algaeSubsystem, @@ -348,6 +351,7 @@ public void toPlaceCoral() { public void toAlgaeFloorPickup() { algaeSubsystem.intake(); + currentAlgaeHeight = algaeHeight; switch (algaeHeight) { case LOW -> { @@ -374,8 +378,13 @@ public void toAlgaeFloorPickup() { } public void toScoreAlgae() { - if (curState == RobotStates.BARGE_ALGAE || curState == RobotStates.PROCESSOR_ALGAE) { - toStow(); + currentAlgaeHeight = algaeHeight; + if (curState == RobotStates.BARGE_ALGAE && algaeHeight == AlgaeHeight.LOW) { + futureState = RobotStates.PROCESSOR_ALGAE; + toStowSafe(); + } else if (curState == RobotStates.PROCESSOR_ALGAE && algaeHeight == AlgaeHeight.HIGH) { + futureState = RobotStates.BARGE_ALGAE; + toStowSafe(); } switch (algaeHeight) { case LOW -> { @@ -390,18 +399,23 @@ public void toScoreAlgae() { double poseX = driveSubsystem.getPoseMeters().getX(); - if (poseX > RobotStateConstants.kRedBargeSafeX - || poseX < RobotStateConstants.kBlueBargeSafeX) { - biscuitSubsystem.setPosition(BiscuitConstants.kBargeBackwardSetpoint); + isBargeSafe = + poseX > RobotStateConstants.kRedBargeSafeX + || poseX < RobotStateConstants.kBlueBargeSafeX; + + if (isBargeSafe) { elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); - setState(RobotStates.BARGE_ALGAE, true); + setState(RobotStates.TO_BARGE_ALGAE); } } } } public void releaseAlgae() { + scoringTimer.stop(); + scoringTimer.reset(); + scoringTimer.start(); switch (algaeHeight) { case LOW -> { algaeSubsystem.scoreProcessor(); @@ -415,6 +429,7 @@ public void releaseAlgae() { } public void toHpAlgae() { + currentAlgaeHeight = algaeHeight; if (needSafeAlgaeTransfer(RobotStates.HP_ALGAE)) { return; } @@ -428,6 +443,7 @@ public void toHpAlgae() { } private void toProcessor() { + currentAlgaeHeight = algaeHeight; if (needSafeAlgaeTransfer(RobotStates.PROCESSOR_ALGAE)) { return; } @@ -478,6 +494,7 @@ public void periodic() { Logger.recordOutput("RobotState/hasCoral", hasCoral()); Logger.recordOutput("RobotState/scoringLevel", scoringLevel); Logger.recordOutput("RobotState/algeaHeight", algaeHeight); + Logger.recordOutput("RobotState/algeaLevelSideBased", getAlgaeLevel()); Logger.recordOutput("RobotState/getAlgea", getAlgaeOnCycle); switch (curState) { @@ -584,12 +601,20 @@ public void periodic() { } case L3, L4 -> { if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { - toReefAlign(false, false); + if (coralSubsystem.hasCoral()) { + toReefAlign(false, false); + } else { + toStowSafe(); + } } } } } else if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { - toReefAlign(false, false); + if (coralSubsystem.hasCoral()) { + toReefAlign(false, false); + } else { + toStowSafe(); + } } } @@ -600,7 +625,11 @@ public void periodic() { } case SAFE_REMOVE_ALGAE_ROTATE -> { - toReefAlign(false, false); + if (coralSubsystem.hasCoral()) { + toReefAlign(false, false); + } else { + toStowSafe(); + } } case PLACE_CORAL -> { @@ -637,23 +666,53 @@ public void periodic() { } } case PROCESSOR_ALGAE -> { - if (!algaeSubsystem.hasAlgae()) { + if (!algaeSubsystem.hasAlgae() + && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) { toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toScoreAlgae(); + } + } + case TO_BARGE_ALGAE -> { + if (elevatorSubsystem.isHigherThan(ElevatorConstants.kBargeHigherThan)) { + Angle BiscuitSetpoint = ElevatorConstants.kBargeSetpoint; + if (driveSubsystem.getPoseMeters().getX() <= DriveConstants.kCenterLineX) { + double yaw = driveSubsystem.getPoseMeters().getRotation().getDegrees(); + BiscuitSetpoint = + yaw < 90 && yaw > -90 + ? BiscuitConstants.kBargeSetpoint + : BiscuitConstants.kBargeBackwardSetpoint; + } else if (driveSubsystem.getPoseMeters().getX() > DriveConstants.kCenterLineX) { + double yaw = driveSubsystem.getPoseMeters().getRotation().getDegrees(); + BiscuitSetpoint = + yaw < -90 || yaw > 90 + ? BiscuitConstants.kBargeSetpoint + : BiscuitConstants.kBargeBackwardSetpoint; + } + biscuitSubsystem.setPosition(BiscuitSetpoint); + curState = RobotStates.BARGE_ALGAE; } } case BARGE_ALGAE -> { - if (!algaeSubsystem.hasAlgae()) { + if (!algaeSubsystem.hasAlgae() + && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) { toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toScoreAlgae(); } } case MIC_ALGAE -> { if (algaeSubsystem.hasAlgae()) { toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toAlgaeFloorPickup(); } } case FLOOR_ALGAE -> { if (algaeSubsystem.hasAlgae()) { toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toAlgaeFloorPickup(); } } case PREP_CLIMB -> {} @@ -690,6 +749,7 @@ public enum RobotStates { PRESTAGE, HP_ALGAE, PROCESSOR_ALGAE, + TO_BARGE_ALGAE, BARGE_ALGAE, MIC_ALGAE, FLOOR_ALGAE, diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index 975916e8..b1be2433 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "25.0.2", + "version": "25.0.4", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "25.0.2" + "version": "25.0.4" } ], "jniDependencies": [], From 8a1200da01cb45b281cd590d7722a6e1c0508218 Mon Sep 17 00:00:00 2001 From: David Shen Date: Wed, 19 Feb 2025 21:19:52 -0500 Subject: [PATCH 15/16] algae everything --- src/main/java/frc/robot/RobotContainer.java | 63 ++++++++++++- .../commands/algae/IntakeAlgaeCommand.java | 17 ++++ .../commands/algae/ProcessorAlgaeCommand.java | 17 ++++ .../frc/robot/constants/AlgaeConstants.java | 72 ++++++++------- .../frc/robot/constants/BiscuitConstants.java | 22 ++--- .../robot/constants/ElevatorConstants.java | 16 ++-- .../frc/robot/subsystems/algae/AlgaeIO.java | 7 +- .../frc/robot/subsystems/algae/AlgaeIOFX.java | 45 +++++----- .../subsystems/algae/AlgaeSubsystem.java | 66 ++++++++------ .../robotState/RobotStateSubsystem.java | 88 +++++++++---------- 10 files changed, 265 insertions(+), 148 deletions(-) create mode 100644 src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java create mode 100644 src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 81166bfc..a1f618b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.algae.IntakeAlgaeCommand; import frc.robot.commands.algae.OpenLoopAlgaeCommand; +import frc.robot.commands.algae.ProcessorAlgaeCommand; import frc.robot.commands.algae.ToggleHasAlgaeCommand; import frc.robot.commands.biscuit.HoldBiscuitCommand; import frc.robot.commands.biscuit.JogBiscuitCommand; @@ -156,6 +158,7 @@ public RobotContainer() { configureTelemetry(); configureDriverBindings(); configureOperatorBindings(); + // configureTestOperatorBindings(); configurePitDashboard(); } @@ -223,6 +226,30 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { + // Move biscuit + new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + + // Move elevator + new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + // Set Levels new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband) .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1)); @@ -260,6 +287,35 @@ private void configureOperatorBindings() { } private void configureTestOperatorBindings() { + new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value) + .onTrue(new IntakeAlgaeCommand(algaeSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kRightBumper.value) + .onTrue(new ProcessorAlgaeCommand(algaeSubsystem)); + + // Move biscuit + new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + + // Move elevator + new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + // Stop Coral new JoystickButton(xboxController, XboxController.Button.kB.value) .onTrue(new OpenLoopCoralCommand(coralSubsystem, 0)); @@ -331,9 +387,14 @@ private void configureTestOperatorBindings() { public void configurePitDashboard() { Shuffleboard.getTab("Pit") - .add("Toggle HasAlgae", new ToggleHasAlgaeCommand(algaeSubsystem)) + .add("Toggle Has Algae", new ToggleHasAlgaeCommand(algaeSubsystem)) .withPosition(3, 2) .withSize(1, 1); + + Shuffleboard.getTab("Pit") + .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem)) + .withPosition(2, 1) + .withSize(1, 1); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java new file mode 100644 index 00000000..2b94e06e --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; + +public class IntakeAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public IntakeAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + } + + @Override + public void initialize() { + algaeSubsystem.intake(); + } +} diff --git a/src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java new file mode 100644 index 00000000..101bf221 --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; + +public class ProcessorAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public ProcessorAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + } + + @Override + public void initialize() { + algaeSubsystem.scoreProcessor(); + } +} diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 71e31cd3..45b3c0f5 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -1,48 +1,52 @@ package frc.robot.constants; -import static edu.wpi.first.units.Units.RotationsPerSecond; - +import com.ctre.phoenix6.configs.CommutationConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; -import edu.wpi.first.units.measure.AngularVelocity; public class AlgaeConstants { public static final int kFxId = 30; - public static final AngularVelocity kCloseEnough = RotationsPerSecond.of(0.1); - public static final AngularVelocity kMaxFwd = RotationsPerSecond.of(100); - public static final AngularVelocity kMaxRev = RotationsPerSecond.of(-100); + public static final double kCloseEnough = 0.1; + public static final double kMaxFwd = 100; + public static final double kMaxRev = -100; + + public static final double kHoldSpeed = -0.1; + public static final double kBargeScoreSpeed = 1; + public static final double kProcessorScoreSpeed = 1; + public static final double kIntakingSpeed = -1; - public static final AngularVelocity kHoldSpeed = RotationsPerSecond.of(-0.1); - public static final AngularVelocity kBargeScoreSpeed = RotationsPerSecond.of(1); - public static final AngularVelocity kProcessorScoreSpeed = RotationsPerSecond.of(1); - public static final AngularVelocity kIntakingSpeed = RotationsPerSecond.of(-1); + public static final double kHasAlgaeVelThreshold = 10; + public static final double kHasAlgaeCounts = 1; // Example Talon FX Config - public static TalonFXConfiguration getFXConfig() { - TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + public static TalonFXSConfiguration getFXConfig() { + TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration(); CurrentLimitsConfigs current = new CurrentLimitsConfigs() - .withStatorCurrentLimit(10) - .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(20) + .withStatorCurrentLimit(40) + .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(10) - .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLowerLimit(2) + .withSupplyCurrentLowerTime(1) .withSupplyCurrentLimitEnable(true); - fxConfig.CurrentLimits = current; + fxsConfig.CurrentLimits = current; HardwareLimitSwitchConfigs hwLimit = new HardwareLimitSwitchConfigs() @@ -54,15 +58,13 @@ public static TalonFXConfiguration getFXConfig() { .withReverseLimitEnable(false) .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); - fxConfig.HardwareLimitSwitch = hwLimit; + fxsConfig.HardwareLimitSwitch = hwLimit; SoftwareLimitSwitchConfigs swLimit = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withForwardSoftLimitThreshold(kMaxFwd.in(RotationsPerSecond)) - .withReverseSoftLimitEnable(true) - .withReverseSoftLimitThreshold(kMaxRev.in(RotationsPerSecond)); - fxConfig.SoftwareLimitSwitch = swLimit; + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); + fxsConfig.SoftwareLimitSwitch = swLimit; Slot0Configs slot0 = new Slot0Configs() @@ -74,7 +76,7 @@ public static TalonFXConfiguration getFXConfig() { .withKS(0) .withKV(0) .withKA(0); - fxConfig.Slot0 = slot0; + fxsConfig.Slot0 = slot0; MotionMagicConfigs motionMagic = new MotionMagicConfigs() @@ -83,14 +85,24 @@ public static TalonFXConfiguration getFXConfig() { .withMotionMagicExpo_kA(0) .withMotionMagicExpo_kV(0) .withMotionMagicJerk(0); - fxConfig.MotionMagic = motionMagic; + fxsConfig.MotionMagic = motionMagic; MotorOutputConfigs motorOut = new MotorOutputConfigs() .withDutyCycleNeutralDeadband(0.01) - .withNeutralMode(NeutralModeValue.Coast); - fxConfig.MotorOutput = motorOut; + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + fxsConfig.MotorOutput = motorOut; + + CommutationConfigs commutationConfigs = + new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST); + fxsConfig.Commutation = commutationConfigs; + + ExternalFeedbackConfigs externalFeedbackConfigs = + new ExternalFeedbackConfigs() + .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + fxsConfig.ExternalFeedback = externalFeedbackConfigs; - return fxConfig; + return fxsConfig; } } diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index a90d15db..a7f4bc0d 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -24,10 +24,10 @@ public class BiscuitConstants { - public static double kZero = .37; - public static double kTicksPerRot = 160; - public static int talonID = 25; - public static double kCloseEnough = 0.05; + public static final double kZero = .36; + public static final double kTicksPerRot = 160; + public static final int talonID = 25; + public static final double kCloseEnough = 0.05; public static final Angle kMaxFwd = Rotations.of(51.04735 + 5); public static final Angle kMaxRev = Rotations.of(-12.3489 - 5); public static final double kSafeToStowUpper = 40; @@ -35,16 +35,16 @@ public class BiscuitConstants { // Setpoints // Idle - public static final Angle kStowSetpoint = Rotations.of(0.0); + public static final Angle kStowSetpoint = Rotations.of(1.862); public static final Angle kFunnelSetpoint = kStowSetpoint; public static final Angle kPrestageSetpoint = kStowSetpoint; // Algae removal - public static final Angle kL2AlgaeSetpoint = Rotations.of(19.11865); - public static final Angle kL3AlgaeSetpoint = Rotations.of(19.11865); + public static final Angle kL2AlgaeSetpoint = Rotations.of(20.848); + public static final Angle kL3AlgaeSetpoint = Rotations.of(24.562); - public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(19.11865); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(19.11865); + public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint; + public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint; public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); @@ -56,12 +56,12 @@ public class BiscuitConstants { public static final Angle kL4CoralSetpoint = kStowSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(51.04735); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(49.627); public static final Angle kMicAlgaeSetpoint = Rotations.of(51.61872); public static final Angle kHpAlgaeSetpoint = Rotations.of(16.97559); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(50.88037); + public static final Angle kProcessorSetpoint = Rotations.of(41.193); public static final Angle kBargeSetpoint = Rotations.of(12.3489); public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 27da7482..f78db9f1 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -44,13 +44,13 @@ public class ElevatorConstants { // Algae removal public static final Angle kL2AlgaeSetpoint = Rotations.of(2.8677); - public static final Angle kL3AlgaeSetpoint = Rotations.of(14.724); + public static final Angle kL3AlgaeSetpoint = Rotations.of(17.513); - public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(2.8677); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(14.724); + public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint; + public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint; public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(2.8677); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(14.724); + public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(15.542); // Coral score public static final Angle kL1CoralSetpoint = Rotations.of(13.04053); @@ -62,13 +62,13 @@ public class ElevatorConstants { public static final Angle kPrestageSetpoint = kL3CoralSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(8.81836); - public static final Angle kMicAlgaeSetpoint = Rotations.of(17.708); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(6.66); + public static final Angle kMicAlgaeSetpoint = Rotations.of(17.07); public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(14.9063); - public static final Angle kBargeSetpoint = Rotations.of(40.913); + public static final Angle kProcessorSetpoint = Rotations.of(4.297); + public static final Angle kBargeSetpoint = Rotations.of(41.936); // 40.913 public static final Angle kBargeHigherThan = Rotations.of(31.0901); public static TalonFXConfiguration getBothFXConfig() { diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java index 52a70bd4..7d7ecdbd 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -1,19 +1,18 @@ package frc.robot.subsystems.algae; -import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; public interface AlgaeIO { @AutoLog public static class AlgaeIOInputs { - public AngularVelocity velocity; - public boolean isLimitSwitchClosed; + public double velocity; + public boolean isBeamBroken; } public default void updateInputs(AlgaeIOInputs inputs) {} - public default void setSpeed(AngularVelocity speed) {} + public default void setSpeed(double speed) {} public default void setPct(double pct) {} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java index 6ead7a35..c185760a 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java @@ -1,23 +1,27 @@ package frc.robot.subsystems.algae; -import static edu.wpi.first.units.Units.RotationsPerSecond; - +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.ReverseLimitValue; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.AlgaeConstants; import org.slf4j.Logger; import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; public class AlgaeIOFX implements AlgaeIO { private Logger logger; - private TalonFX talonFX; + private TalonFXS talonFXS; private AlgaeIOInputs inputs; + private TalonFXSConfigurator configurator; + // FX Access objects private StatusSignal curVelocity; private StatusSignal fwdLimitSwitch; @@ -27,37 +31,36 @@ public class AlgaeIOFX implements AlgaeIO { public AlgaeIOFX() { logger = LoggerFactory.getLogger(this.getClass()); - // talonFX = new TalonFX(AlgaeConstants.kFxId); - // fwdLimitSwitch = talonFX.getForwardLimit(); - // revLimitSwitch = talonFX.getReverseLimit(); - // curVelocity = talonFX.getVelocity(); + talonFXS = new TalonFXS(AlgaeConstants.kFxId); + + configurator = talonFXS.getConfigurator(); + configurator.apply(new TalonFXSConfiguration()); // Factory default motor controller + configurator.apply(AlgaeConstants.getFXConfig()); + + fwdLimitSwitch = talonFXS.getForwardLimit(); + revLimitSwitch = talonFXS.getReverseLimit(); + curVelocity = talonFXS.getVelocity(); } @Override public void updateInputs(AlgaeIOInputs inputs) { - // BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); - // inputs.velocity = curVelocity.refresh().getValue(); - // inputs.isLimitSwitchClosed = fwdLimitSwitch.getValue().value == 1; // FIXME check right value - inputs.velocity = RotationsPerSecond.of(0); - inputs.isLimitSwitchClosed = true; + BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); + inputs.velocity = curVelocity.getValueAsDouble(); + inputs.isBeamBroken = fwdLimitSwitch.getValue().value == 0; } @Override - public void setSpeed(AngularVelocity speed) { - talonFX.setControl(speedRequest.withVelocity(speed)); + public void setSpeed(double speed) { + talonFXS.setControl(speedRequest.withVelocity(speed)); } @Override public void setPct(double pct) { - talonFX.setControl(dutyCycleRequest.withOutput(pct)); - } - - public AngularVelocity AngularVelocity() { - return talonFX.getVelocity().getValue(); + talonFXS.setControl(dutyCycleRequest.withOutput(pct)); } @Override public void registerWith(TelemetryService telemetryService) { - // telemetryService.register(talonFX, true); + telemetryService.register(talonFXS, true); } } diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java index 18da2276..4628be7a 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -1,24 +1,21 @@ package frc.robot.subsystems.algae; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.constants.AlgaeConstants; -import frc.robot.standards.ClosedLoopSpeedSubsystem; -import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs; import java.util.Set; +import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; -public class AlgaeSubsystem extends MeasurableSubsystem implements ClosedLoopSpeedSubsystem { +public class AlgaeSubsystem extends MeasurableSubsystem { private org.slf4j.Logger logger = LoggerFactory.getLogger(AlgaeSubsystem.class); private final AlgaeIO io; - private final AlgaeIOInputs inputs = new AlgaeIOInputs(); - private AngularVelocity desiredSpeed = RotationsPerSecond.of(0); + private final AlgaeIOInputsAutoLogged inputs = new AlgaeIOInputsAutoLogged(); + private double desiredSpeed = 0; + private double slowCounts = 0; private AlgaeStates curState = AlgaeStates.EMPTY; @@ -36,64 +33,75 @@ public void setState(AlgaeStates newState) { } public void intake() { - setSpeed(AlgaeConstants.kIntakingSpeed); + // setSpeed(AlgaeConstants.kIntakingSpeed); + setPct(0.5); + slowCounts = 0; } public void scoreProcessor() { - setSpeed(AlgaeConstants.kProcessorScoreSpeed); + // setSpeed(AlgaeConstants.kProcessorScoreSpeed); + setPct(-1); } public void scoreBarge() { - setSpeed(AlgaeConstants.kBargeScoreSpeed); + // setSpeed(AlgaeConstants.kBargeScoreSpeed); + setPct(-1); } public void hold() { - setSpeed(AlgaeConstants.kHoldSpeed); + // setSpeed(AlgaeConstants.kHoldSpeed); + setPct(-0); } public boolean hasAlgae() { return curState == AlgaeStates.HAS_ALGAE; } - @Override - public void setSpeed(AngularVelocity speed) { + public void setSpeed(double speed) { // io.setSpeed(speed); // desiredSpeed = speed; } public void setPct(double pct) { - // io.setPct(pct); + io.setPct(pct); } - @Override - public AngularVelocity getSpeed() { + public double getSpeed() { return inputs.velocity; } - @Override public boolean atSpeed() { - return inputs.velocity.minus(desiredSpeed).abs(RotationsPerSecond) - < AlgaeConstants.kCloseEnough.in(RotationsPerSecond); + return FastMath.abs(inputs.velocity - desiredSpeed) < AlgaeConstants.kCloseEnough; } @Override public void periodic() { io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); Logger.recordOutput("Algae/state", curState); - Logger.recordOutput("Algae/setpoint", desiredSpeed.in(RotationsPerSecond)); + Logger.recordOutput("Algae/setpoint", desiredSpeed); switch (curState) { case HAS_ALGAE -> { - // if (!inputs.isLimitSwitchClosed) { - // setState(AlgaeStates.EMPTY); - // setSpeed(RotationsPerSecond.of(0)); - // } + if (!inputs.isBeamBroken) { + setState(AlgaeStates.EMPTY); + // setPct(0); + // setSpeed(RotationsPerSecond.of(0)); + } } case EMPTY -> { - // if (inputs.isLimitSwitchClosed) { // FIXME: correct? - // hold(); - // setState(AlgaeStates.HAS_ALGAE); - // } + if (inputs.isBeamBroken) { + if (FastMath.abs(inputs.velocity) < AlgaeConstants.kHasAlgaeVelThreshold) { + slowCounts++; + } else { + slowCounts = 0; + } + + if (slowCounts >= AlgaeConstants.kHasAlgaeCounts) { + hold(); + setState(AlgaeStates.HAS_ALGAE); + } + } } case IDLE -> {} } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index abde5a6a..945e271b 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -60,6 +60,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private boolean isAuto = true; private Timer scoringTimer = new Timer(); + private boolean isEjectingAlgae = false; private Pose2d algaeRemovalPose; public boolean isBargeSafe = true; @@ -240,22 +241,34 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) { } public void toStow() { - biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); - driveSubsystem.removeDriveMultiplier(); - // algaeSubsystem.hold(); + if (biscuitSubsystem.isSafeToStow()) { + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); + algaeSubsystem.hold(); - setState(RobotStates.TO_STOW); + setState(RobotStates.TO_STOW); + } else { + toStowSafe(); + } } public void toStowSafe() { biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); driveSubsystem.removeDriveMultiplier(); - // algaeSubsystem.hold(); + algaeSubsystem.hold(); setState(RobotStates.TO_STOW_SAFE); } + public void toStowSequential() { + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); + algaeSubsystem.hold(); + + setState(RobotStates.TO_STOW_SEQUENTIAL); + } + public void toPrepCoral() { if (curState == RobotStates.REEF_ALIGN_CORAL) { if (elevatorSubsystem.isFinished()) { @@ -284,7 +297,8 @@ private void toReefAlign(boolean getAlgae, boolean drive) { if (!coralSubsystem.hasCoral()) { getAlgae = true; } - if (getAlgae && scoreSide == ScoreSide.LEFT) { + if ((getAlgae || !coralSubsystem.hasCoral()) + && (scoreSide == ScoreSide.LEFT || !isAutoPlacing)) { if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) { return; } @@ -416,6 +430,7 @@ public void releaseAlgae() { scoringTimer.stop(); scoringTimer.reset(); scoringTimer.start(); + isEjectingAlgae = true; switch (algaeHeight) { case LOW -> { algaeSubsystem.scoreProcessor(); @@ -519,6 +534,13 @@ public void periodic() { } } + case TO_STOW_SEQUENTIAL -> { + if (biscuitSubsystem.isFinished()) { + elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + setState(RobotStates.TO_STOW); + } + } + case STOW -> { if (futureState != null) { switch (futureState) { @@ -554,14 +576,6 @@ public void periodic() { case L2 -> { biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint); - - // // FIXME: if driving to remove algae - // switch (scoringLevel) { - // case L1, L2 -> { - // driveSubsystem.move(DriveConstants.kAlgaeRemovalSpeed, 0, 0, false); - // } - // default -> {} - // } } case L3 -> { biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint); @@ -570,7 +584,7 @@ public void periodic() { default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); } - setState(RobotStates.REMOVE_ALGAE); + setState(RobotStates.REMOVE_ALGAE, true); } } } @@ -585,36 +599,17 @@ public void periodic() { } case REMOVE_ALGAE -> { if (getAlgaeLevel() == ScoringLevel.L2) { - switch (scoringLevel) { - case L1, L2 -> { - // // FIXME: if driving to remove algae - // if - // (driveSubsystem.getPoseMeters().minus(algaeRemovalPose).getTranslation().getNorm() - // > RobotStateConstants.kAlgaeRetreatDistance) { - // driveSubsystem.move(0, 0, 0, false); - // toReefAlign(false, true); - // } - // // FIXME: if not driving to remove algae - biscuitSubsystem.setPosition(BiscuitConstants.kSafeAlgaeRemovalSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kSafeAlgaeRemovalSetpoint); - setState(RobotStates.SAFE_REMOVE_ALGAE_ABOVE, true); - } - case L3, L4 -> { - if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { - if (coralSubsystem.hasCoral()) { - toReefAlign(false, false); - } else { - toStowSafe(); - } - } - } - } - } else if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { + if (coralSubsystem.hasCoral()) { toReefAlign(false, false); } else { - toStowSafe(); + toStowSequential(); } + + } else if (coralSubsystem.hasCoral()) { + toReefAlign(false, false); + } else { + toStowSequential(); } } @@ -667,7 +662,9 @@ public void periodic() { } case PROCESSOR_ALGAE -> { if (!algaeSubsystem.hasAlgae() - && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) { + && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer) + && isEjectingAlgae) { + isEjectingAlgae = false; toStowSafe(); } else if (algaeHeight != currentAlgaeHeight) { toScoreAlgae(); @@ -695,7 +692,9 @@ public void periodic() { } case BARGE_ALGAE -> { if (!algaeSubsystem.hasAlgae() - && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) { + && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer) + && isEjectingAlgae) { + isEjectingAlgae = false; toStowSafe(); } else if (algaeHeight != currentAlgaeHeight) { toScoreAlgae(); @@ -737,6 +736,7 @@ public enum RobotStates { STOW, TO_STOW, TO_STOW_SAFE, + TO_STOW_SEQUENTIAL, PREP_CORAL, REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, From 80d290fd0d2f9a5c4007b3cd0bdd26c07c0b6816 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 20 Feb 2025 19:55:51 -0500 Subject: [PATCH 16/16] made algea and coral sequencing at the reef work --- .../commands/robotState/ReefCycleCommand.java | 2 +- .../robotState/ScoreReefManualCommand.java | 2 +- .../robotState/ToggleAutoCommand.java | 6 +- .../frc/robot/constants/BiscuitConstants.java | 7 +- .../robot/constants/ElevatorConstants.java | 3 - .../subsystems/pathHandler/PathHandler.java | 2 + .../robotState/RobotStateSubsystem.java | 122 ++++++------------ 7 files changed, 51 insertions(+), 93 deletions(-) diff --git a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java index 1ea8acea..4b0695bb 100644 --- a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java @@ -30,7 +30,7 @@ public ReefCycleCommand( public void initialize() { startingRobotState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); - isAutoPlacing = robotStateSubsystem.getIsAuto(); + isAutoPlacing = robotStateSubsystem.getAutoPlaceOnCycle(); robotStateSubsystem.toPrepCoral(); } diff --git a/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java index e792f64e..700f5638 100644 --- a/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java @@ -25,7 +25,7 @@ public ScoreReefManualCommand( public void initialize() { startingRobotState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); - robotStateSubsystem.setIsAuto(false); + robotStateSubsystem.setAutoPlaceOnCycle(false); robotStateSubsystem.setGetAlgaeOnCycle(false); robotStateSubsystem.toPrepCoral(); } diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java index f65b3dd5..3199458c 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java @@ -12,10 +12,10 @@ public ToggleAutoCommand(RobotStateSubsystem robotState) { @Override public void initialize() { - if (robotState.getIsAuto() == false) { - robotState.setIsAuto(true); + if (robotState.getAutoPlaceOnCycle() == false) { + robotState.setAutoPlaceOnCycle(true); } else { - robotState.setIsAuto(false); + robotState.setAutoPlaceOnCycle(false); } } } diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index a7f4bc0d..e408d970 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -43,11 +43,8 @@ public class BiscuitConstants { public static final Angle kL2AlgaeSetpoint = Rotations.of(20.848); public static final Angle kL3AlgaeSetpoint = Rotations.of(24.562); - public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint; - public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint; - - public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0); + public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0); // Coral score public static final Angle kL1CoralSetpoint = kStowSetpoint; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index f78db9f1..13ce04c0 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -49,9 +49,6 @@ public class ElevatorConstants { public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint; public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint; - public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(2.8677); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(15.542); - // Coral score public static final Angle kL1CoralSetpoint = Rotations.of(13.04053); public static final Angle kL2CoralSetpoint = Rotations.of(21.0786); // 19.62793 -> 21.0786 diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 9d2f6de4..c60ad888 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -90,6 +90,7 @@ public void setStartNode(Character startNode) { public void startPathHandler() { nodeNames.add(0, startNode); isHandling = true; + robotStateSubsystem.setAutoPlaceOnCycle(false); curState = PathStates.DRIVE_FETCH; } @@ -189,6 +190,7 @@ public void killPathHandler() { isHandling = false; curState = PathStates.DONE; runningPath = false; + robotStateSubsystem.setAutoPlaceOnCycle(true); timer.stop(); timer.reset(); } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 945e271b..a34b19a9 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -46,6 +46,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private RobotStates curState = RobotStates.IDLE; private RobotStates nextState; private RobotStates futureState; + private boolean isAuto = false; private ScoringLevel scoringLevel = ScoringLevel.L4; private ScoringLevel currentLevel = ScoringLevel.L4; @@ -57,7 +58,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private boolean isAutoPlacing = false; private boolean getAlgaeOnCycle = false; private boolean isCurrentLimiting = false; - private boolean isAuto = true; + private boolean autoPlaceOnCycle = false; private Timer scoringTimer = new Timer(); private boolean isEjectingAlgae = false; @@ -162,7 +163,7 @@ public void toggleAlgaeHeight() { algaeHeight = algaeHeight == AlgaeHeight.LOW ? AlgaeHeight.HIGH : AlgaeHeight.LOW; } - public void setAutoPlacing(boolean isAutoPlacing) { + public void setIsAutoPlacing(boolean isAutoPlacing) { this.isAutoPlacing = isAutoPlacing; } @@ -182,12 +183,12 @@ public void setCurrentLimiting(boolean isCurrentLimiting) { this.isCurrentLimiting = isCurrentLimiting; } - public void setIsAuto(boolean isAuto) { - this.isAuto = isAuto; + public void setAutoPlaceOnCycle(boolean isAuto) { + this.autoPlaceOnCycle = isAuto; } - public boolean getIsAuto() { - return isAuto; + public boolean getAutoPlaceOnCycle() { + return autoPlaceOnCycle; } public void setAutoPlacingLed(boolean isAutoPlacing) { @@ -214,13 +215,7 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) { } switch (curState) { - case REEF_ALIGN_ALGAE, - REEF_ALIGN_CORAL, - REMOVE_ALGAE, - SAFE_REMOVE_ALGAE_ABOVE, - SAFE_REMOVE_ALGAE_ROTATE, - PLACE_CORAL, - INTERRUPTED -> { + case REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, REMOVE_ALGAE, PLACE_CORAL, INTERRUPTED -> { switch (nextState) { // These are all the states that could possibly be entered that are also // dangerous @@ -274,10 +269,8 @@ public void toPrepCoral() { if (elevatorSubsystem.isFinished()) { toPlaceCoral(); } - } else if (isAuto) { - toReefAlign(false, false); } else { - toReefAlign(getAlgaeOnCycle, false); // Manual scoring + toReefAlign(getAlgaeOnCycle, autoPlaceOnCycle); } } @@ -294,11 +287,9 @@ public void toReefAlign() { } private void toReefAlign(boolean getAlgae, boolean drive) { - if (!coralSubsystem.hasCoral()) { - getAlgae = true; - } if ((getAlgae || !coralSubsystem.hasCoral()) - && (scoreSide == ScoreSide.LEFT || !isAutoPlacing)) { + && (scoreSide == ScoreSide.LEFT || !autoPlaceOnCycle) + && !algaeSubsystem.hasAlgae()) { if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) { return; } @@ -318,33 +309,28 @@ private void toReefAlign(boolean getAlgae, boolean drive) { default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); } } else { - if (!coralSubsystem.hasCoral()) { - toStow(); + if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_CORAL)) { return; - } else { - if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_CORAL)) { - return; - } - setState(RobotStates.REEF_ALIGN_CORAL); + } + setState(RobotStates.REEF_ALIGN_CORAL); - currentLevel = scoringLevel; - switch (scoringLevel) { - case L1 -> { - biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); - } - case L2 -> { - biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); - } - case L3 -> { - biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); - } - case L4 -> { - biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); - } + currentLevel = scoringLevel; + switch (scoringLevel) { + case L1 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); + } + case L2 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); + } + case L3 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); + } + case L4 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); } } } @@ -544,8 +530,8 @@ public void periodic() { case STOW -> { if (futureState != null) { switch (futureState) { - case REEF_ALIGN_ALGAE -> toReefAlign(true, isAutoPlacing); - case REEF_ALIGN_CORAL -> toReefAlign(false, isAutoPlacing); + case REEF_ALIGN_ALGAE -> toReefAlign(true, autoPlaceOnCycle); + case REEF_ALIGN_CORAL -> toReefAlign(false, autoPlaceOnCycle); case HP_ALGAE -> toHpAlgae(); case PROCESSOR_ALGAE, BARGE_ALGAE -> toScoreAlgae(); case MIC_ALGAE, FLOOR_ALGAE -> toAlgaeFloorPickup(); @@ -567,11 +553,9 @@ public void periodic() { // } // } case REEF_ALIGN_ALGAE -> { - if (!isAutoPlacing + if (!autoPlaceOnCycle || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { if (algaeSubsystem.hasAlgae()) { - algaeRemovalPose = driveSubsystem.getPoseMeters(); - switch (getAlgaeLevel()) { case L2 -> { biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint); @@ -584,46 +568,26 @@ public void periodic() { default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); } - setState(RobotStates.REMOVE_ALGAE, true); + setState(RobotStates.REMOVE_ALGAE); } } } case REEF_ALIGN_CORAL -> { - if (tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { - if (isAutoPlacing) { - toPlaceCoral(); - } else if (currentLevel != scoringLevel) { - toReefAlign(getAlgaeOnCycle, false); - } + if (currentLevel != scoringLevel) { + toReefAlign(false, false); + } + if (autoPlaceOnCycle + && tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { + toPlaceCoral(); } } case REMOVE_ALGAE -> { - if (getAlgaeLevel() == ScoringLevel.L2) { - + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { if (coralSubsystem.hasCoral()) { toReefAlign(false, false); } else { toStowSequential(); } - - } else if (coralSubsystem.hasCoral()) { - toReefAlign(false, false); - } else { - toStowSequential(); - } - } - - case SAFE_REMOVE_ALGAE_ABOVE -> { - biscuitSubsystem.setPosition(BiscuitConstants.kSafeAlgaeRemovalRotateSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kSafeAlgaeRemovalRotateSetpoint); - setState(RobotStates.SAFE_REMOVE_ALGAE_ROTATE, true); - } - - case SAFE_REMOVE_ALGAE_ROTATE -> { - if (coralSubsystem.hasCoral()) { - toReefAlign(false, false); - } else { - toStowSafe(); } } @@ -741,8 +705,6 @@ public enum RobotStates { REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, REMOVE_ALGAE, - SAFE_REMOVE_ALGAE_ABOVE, - SAFE_REMOVE_ALGAE_ROTATE, PLACE_CORAL, FUNNEL_LOAD, LOADING_CORAL,