Match prep (#20)

* added matchPrepCommand * added matchPrepCommand
ExcaliburFRC · Jan 25, 2024 · bedfbff · bedfbff
1 parent c1ebb85
commit bedfbff
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -15,8 +15,7 @@
 import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kForward;
 import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kReverse;
 import static frc.lib.Color.Colors.WHITE;
-import static frc.robot.Constants.IntakeConstants.INTAKE_ANGLE.GROUND;
-import static frc.robot.Constants.IntakeConstants.INTAKE_ANGLE.HUMAN_PLAYER;
+import static frc.robot.Constants.IntakeConstants.INTAKE_ANGLE.*;
 import static frc.robot.Constants.ShooterConstants.SPEAKER_PREP_RADIUS;
 import static frc.robot.subsystems.LEDs.LEDPattern.SOLID;
 import static frc.robot.Constants.FieldConstants.FieldLocations.*;
@@ -39,10 +38,11 @@ public class RobotContainer {
     public final Trigger isAtSpeakerRadius = new Trigger(()-> swerve.getDistanceFromPose(SPEAKER_CENTER.pose.get()) < SPEAKER_PREP_RADIUS);
 
     public ShuffleboardTab matchTab = Shuffleboard.getTab("Match settings");
+    public ShuffleboardTab pitTab = Shuffleboard.getTab("pit");
 
     public RobotContainer(){
         configureBindings();
-        initSendableChoosers();
+        initShuffleBoard();
     }
 
     // bindings
@@ -96,6 +96,14 @@ private void configureBindings() {
     }
 
     // methods
+    public Command matchPrepCommand() {
+        return new SequentialCommandGroup(
+                swerve.straightenModulesCommand(),
+                shooter.closeLinearCommand(),
+                intake.setIntakeAngleCommand(SHOOTER)
+        );
+    }
+
     public Command scoreNoteCommand(Command shooterCommand){
         return new ParallelCommandGroup(
                 shooterCommand,
@@ -117,9 +125,12 @@ public Command toggleMotorsIdleMode() {
         );
     }
 
-    private void initSendableChoosers(){
+    private void initShuffleBoard(){
         shouldDriveToCenterLineChooser.setDefaultOption("don't Drive", Commands.none());
         shouldDriveToCenterLineChooser.addOption("drive", Commands.idle()); // this is my commandddd!!
+
+        pitTab.add("Match prep", matchPrepCommand());
+
     }
 
     public Command getAutonomousCommand(){

diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -60,7 +60,7 @@ public Command stallIntakeMotor() {
         });
     }
 
-    private Command setIntakeAngleCommand(INTAKE_ANGLE angle) {
+    public Command setIntakeAngleCommand(INTAKE_ANGLE angle) {
         return new FunctionalCommand(
                 () -> {
                 },
-Original file line number
+Diff line change
@@ Expand Up / @@ -60,7 +60,7 @@ public Command stallIntakeMotor() { @@
             });
         }
-        private Command setIntakeAngleCommand(INTAKE_ANGLE angle) {
+        public Command setIntakeAngleCommand(INTAKE_ANGLE angle) {
             return new FunctionalCommand(
                     () -> {
                     },
@@ Expand Down @@