Added named commands edited piececontrol

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;
@@ -35,6 +41,14 @@ public class RobotContainer implements Logged {
     private final Limelight limelight;
     private final Climb climb;
     private Indexer triggerWheel;
+    private Pivot pivot;
+    private Shooter shooter;
+    private Claw claw;
+    private Elevator elevator;
+
+    private ShooterCalc shooterCalc;
+
+    private PieceControl pieceControl;
 
     public RobotContainer() {
         driver = new PatriBoxController(OIConstants.DRIVER_CONTROLLER_PORT, OIConstants.DRIVER_DEADBAND);
@@ -47,6 +61,14 @@ public RobotContainer() {
         swerve = new Swerve();
         driverUI = new DriverUI();
         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
@@ -126,7 +148,13 @@ public void onEnabled() {
     }
 
     public void prepareNamedCommands() {
-        NamedCommands.registerCommand("Intake", intake.inCommand());
+        // TODO: prepare to shoot while driving (w1 - c1)
+        NamedCommands.registerCommand("intake", intake.inCommand());
+        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() {

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());

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 {