new named commands

Patribots4738 · Jan 31, 2024 · ef152b8 · ef152b8
1 parent 0f9bb09
commit ef152b8
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,7 +13,13 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.commands.Drive;
+import frc.robot.commands.PieceControl;
+import frc.robot.commands.ShooterCalc;
 import frc.robot.subsystems.*;
+import frc.robot.subsystems.elevator.Claw;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.shooter.Pivot;
+import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.util.PatriBoxController;
 import frc.robot.util.PoseCalculations;
 import frc.robot.util.Constants.FieldConstants;
@@ -37,8 +43,12 @@ public class RobotContainer implements Logged {
     private Indexer triggerWheel;
     private Pivot pivot;
     private Shooter shooter;
+    private Claw claw;
+    private Elevator elevator;
 
-    private ShooterCalc shooterCalc;    
+    private ShooterCalc shooterCalc;
+
+    private PieceControl pieceControl;
 
     public RobotContainer() {
         driver = new PatriBoxController(OIConstants.DRIVER_CONTROLLER_PORT, OIConstants.DRIVER_DEADBAND);
@@ -53,9 +63,13 @@ public RobotContainer() {
         triggerWheel = new Indexer();
         pivot = new Pivot();
         shooter = new Shooter();
+        claw = new Claw();
+        elevator = new Elevator();
 
         shooterCalc = new ShooterCalc(shooter, pivot);
 
+        pieceControl = new PieceControl(intake, triggerWheel, elevator, claw, shooterCalc, swerve);
+
         limelight.setDefaultCommand(Commands.run(() -> {
             // Create an "Optional" object that contains the estimated pose of the robot
             // This can be present (sees tag) or not present (does not see tag)
@@ -134,15 +148,13 @@ public void onEnabled() {
     }
 
     public void prepareNamedCommands() {
+        // TODO: prepare to shoot while driving (w1 - c1)
         NamedCommands.registerCommand("intake", intake.inCommand());
-        NamedCommands.registerCommand("Shoot", shooterCalc.prepareFire());
-        NamedCommands.registerCommand("shootWhileDriving", shooterCalc.prepareFireMovingCommand(swerve.getPose()));
-        NamedCommands.registerCommand("placeAMP", placeAMP.placeAMP());
-        NamedCommands.registerCommand("prepareShooterL", shooterCalc.prepareFire(new Pose2d()));
-        NamedCommands.registerCommand("prepareShooterM", shooterCalc.prepareFire(new Pose2d()));
-        NamedCommands.registerCommand("prepareShooterR", shooterCalc.prepareFire(new Pose2d()));
-
-
+        NamedCommands.registerCommand("shoot", pieceControl.noteToShoot());
+        NamedCommands.registerCommand("placeAmp", pieceControl.noteToTarget(() -> true));
+        NamedCommands.registerCommand("prepareShooterL", shooterCalc.prepareFireCommand(() -> true, FieldConstants.L_POSE));
+        NamedCommands.registerCommand("prepareShooterM", shooterCalc.prepareFireCommand(() -> true, FieldConstants.M_POSE));
+        NamedCommands.registerCommand("prepareShooterR", shooterCalc.prepareFireCommand(() -> true, FieldConstants.R_POSE));
     }
 
     private void incinerateMotors() {

diff --git a/src/main/java/frc/robot/commands/PieceControl.java b/src/main/java/frc/robot/commands/PieceControl.java
@@ -1,5 +1,7 @@
 package frc.robot.commands;
 
+import java.util.function.BooleanSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -9,6 +11,7 @@
 import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.elevator.Claw;
 import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.TrapConstants;
 
 public class PieceControl {
@@ -62,15 +65,15 @@ public Command stopAllMotors() {
     }
 
     // TODO: We assume that the note is already in the claw when we want to shoot
+    // TODO: Possibly split this into two commands where one sends to shooter without waiting
     public Command noteToShoot() {
         // this.notePosition = NotePosition.CLAW; ^^^
         Command shoot = emptyCommand;
-        this.shooterCalc.prepareFireMovingCommand(() -> true, swerve);
         if (this.readyToShoot().getAsBoolean()) {
             // run the indexer and intake to make sure the note gets to the shooter
             shoot = indexer.toShooter()
-                    .andThen(intake.inCommand())
-                    .andThen(claw.outtake())
+                    .andThen(Commands.waitUntil(() -> shooterCalc.shooterAtDesiredRPM() && shooterCalc.pivotAtDesiredAngle()))
+                    .andThen(claw.intake())
                     .andThen(() -> this.notePosition = NotePosition.SHOOTER)
                     .andThen(Commands.waitSeconds(1)) // TODO: Change this to a wait until the note is in the shooter?
                     .andThen(() -> this.notePosition = NotePosition.NONE)
@@ -81,14 +84,15 @@ public Command noteToShoot() {
         return shoot;
     }
 
-    public Command noteToTrap() {
+    public Command noteToTarget(BooleanSupplier toAmp) {
         Command shoot = emptyCommand;
 
         if (this.notePosition == NotePosition.CLAW) {
             // maybe make setPosition a command ORR Make the Elevator Command
             shoot = Commands.runOnce(
-                    () -> this.elevator.setPositionCommand(TrapConstants.TRAP_PLACE_POS)).andThen(
-                            Commands.waitUntil(elevator.isAtTargetPosition()))
+                    () -> this.elevator.setPositionCommand(toAmp.getAsBoolean() ? TrapConstants.TRAP_PLACE_POS : TrapConstants.TRAP_PLACE_POS))
+                                            .andThen(
+Commands.waitUntil(elevator.isAtTargetPosition()))
                     .andThen(claw.placeCommand())
                     .andThen(new WaitCommand(1))
                     .andThen(stopAllMotors());

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -423,6 +423,10 @@ public static enum GameMode {
                 new Pose2d(14.706, FIELD_HEIGHT_METERS, Rotation2d.fromDegrees(-90)),
         };
 
+        public static Pose2d L_POSE = new Pose2d();
+        public static Pose2d R_POSE = new Pose2d();
+        public static Pose2d M_POSE = new Pose2d();
+
         public static final double CHAIN_LENGTH_METERS = Units.inchesToMeters(100);
 
         public static enum ChainPosition {