Refactor command groups

java-formatter.xml

@@ -14,7 +14,7 @@
         <setting id="org.eclipse.jdt.core.formatter.insert_space_after_comma_in_constructor_declaration_parameters" value="insert"/>
         <setting id="org.eclipse.jdt.core.formatter.comment.insert_new_line_for_parameter" value="do not insert"/>
         <setting id="org.eclipse.jdt.core.formatter.insert_new_line_after_annotation_on_package" value="insert"/>
-        <setting id="org.eclipse.jdt.core.formatter.parentheses_positions_in_method_invocation" value="common_lines"/>
+        <setting id="org.eclipse.jdt.core.formatter.parentheses_positions_in_method_invocation" value="separate_lines_if_wrapped"/>
         <setting id="org.eclipse.jdt.core.formatter.insert_space_between_empty_parens_in_enum_constant" value="do not insert"/>
         <setting id="org.eclipse.jdt.core.formatter.blank_lines_after_imports" value="1"/>
         <setting id="org.eclipse.jdt.core.formatter.insert_space_before_closing_paren_in_while" value="do not insert"/>
@@ -387,5 +387,7 @@
         <setting id="org.eclipse.jdt.core.formatter.insert_space_before_opening_paren_in_switch" value="do not insert"/>
 
         <setting id="org.eclipse.jdt.core.formatter.lineSplit" value="180"/>
+
+        <setting id="org.eclipse.jdt.core.formatter.wrap_object_initializer_elements" value="true"/>
     </profile>
 </profiles>

src/main/java/frc/robot/RobotContainer.java

@@ -1,8 +1,6 @@
 package frc.robot;
 
 import org.photonvision.PhotonCamera;
-
-import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -13,87 +11,120 @@
 import frc.lib.zylve.Controller;
 import frc.robot.auto.PhotonRunnable;
 import frc.robot.auto.PoseEstimatorSubsystem;
-import frc.robot.auto.commands.AmpScore;
+import frc.robot.auto.commands.AmpAlign;
 import frc.robot.auto.commands.FollowAprilTag;
-import frc.robot.auto.commands.ShootNote;
+import frc.robot.auto.commands.SpeakerAlign;
 import frc.robot.constants.OperatorConstants;
 import frc.robot.elevator.Elevator;
+import frc.robot.elevator.commands.ElevatorDown;
+import frc.robot.elevator.commands.ElevatorUp;
 import frc.robot.intake.Intake;
+import frc.robot.intake.commands.IntakeAmpScore;
+import frc.robot.intake.commands.IntakeShooterFeed;
 import frc.robot.shooter.Shooter;
+import frc.robot.shooter.commands.ShooterSpinDown;
+import frc.robot.shooter.commands.ShooterSpinUp;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.swerve.SwerveDrive;
 
 public class RobotContainer {
     Controller controller = new Controller(OperatorConstants.CONTROLLER_PORT);
-    private final PhotonCamera photonCamera  = new PhotonCamera("Limelight");
+    private final PhotonCamera photonCamera = new PhotonCamera("Limelight");
     private final PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev);
 
     private final SwerveDrive swerveDrive = new SwerveDrive();
-
     private final Elevator elevator = new Elevator();
     private final Intake intake = new Intake();
     private final Shooter shooter = new Shooter();
-
-
+
     private final PhotonRunnable photonRunnable = new PhotonRunnable(photonCamera);
     private final PoseEstimatorSubsystem poseEstimator = new PoseEstimatorSubsystem(swerveDrive::getRotation2d, swerveDrive::getModulePositions, photonRunnable);
     private final FollowAprilTag followAprilTag = new FollowAprilTag(swerveDrive, photonCamera, poseEstimator::getCurrentPose);
 
-    private final AmpScore AmpScore = new AmpScore(elevator, intake);
-    private final ShootNote shootnotefeed = new ShootNote(shooter, intake);
+    private final SequentialCommandGroup ampSuperCommand = new SequentialCommandGroup(
+        new ParallelCommandGroup(
+            new AmpAlign(swerveDrive, poseEstimator::getCurrentPose),
+            new ElevatorUp(elevator)
+        ),
 
-    // @SuppressWarnings("unused")
-    // private final Elevator elevator = new Elevator();
+        new IntakeAmpScore(intake),
+        new ElevatorDown(elevator).withTimeout(0.25)
+    );
 
-    // @SuppressWarnings("unused")
-    // private final Intake intake = new Intake();
+    private final SequentialCommandGroup shooterSuperCommand = new SequentialCommandGroup(
+        new ParallelCommandGroup(
+            new SpeakerAlign(swerveDrive, poseEstimator::getCurrentPose),
+            new ShooterSpinUp(shooter)
+        ),
 
-    // @SuppressWarnings("unused")
-    // private final Shooter shooter = new Shooter();
+        new IntakeShooterFeed(intake),
+        new ShooterSpinDown(shooter)
+    );
 
     SendableChooser<Command> autoSelector = new SendableChooser<>();
 
     public RobotContainer() {
-        configureButtonBindings();
+        configureSubsystems();
+        configureAutonomous();
+        configureSwerveDriveCommands();
+
+        swerveDrive.setDefaultCommand(
+            new RunCommand(
+                () -> swerveDrive.drive(
+                    -MathUtil.applyDeadband(controller.getLeftY(), OperatorConstants.DEADBAND),
+                    -MathUtil.applyDeadband(controller.getLeftX(), OperatorConstants.DEADBAND),
+                    -MathUtil.applyDeadband(controller.getRightX(), OperatorConstants.DEADBAND),
+                    true, true
+                ),
+                swerveDrive
+            )
+        );
+    }
 
+    private void configureSubsystems() {
         PDH.setSwitchableChannel(true);
+    }
 
+    private void configureAutonomous() {
         autoSelector.addOption("Left Auto", new PathPlannerAuto("Left Auto"));
         autoSelector.addOption("Right Auto", new PathPlannerAuto("Right Auto"));
         autoSelector.setDefaultOption("Middle Auto", new PathPlannerAuto("Middle Auto"));
 
-        NamedCommands.registerCommand("Amp Score", AmpScore);
-        NamedCommands.registerCommand("Shoot Note", shootnotefeed);
-
+        // NamedCommands.registerCommand("Amp Score", AmpScore);
+        // NamedCommands.registerCommand("Shoot Note", shootnotefeed);
 
         Shuffleboard.getTab("auto").add(autoSelector);
-
-        swerveDrive.setDefaultCommand(
-            new RunCommand(
-                () -> swerveDrive.drive(
-                    -MathUtil.applyDeadband(controller.getLeftY(), OperatorConstants.DEADBAND),
-                    -MathUtil.applyDeadband(controller.getLeftX(), OperatorConstants.DEADBAND),
-                    -MathUtil.applyDeadband(controller.getRightX(), OperatorConstants.DEADBAND),
-                    true, true),
-                swerveDrive));
     }
 
-    private void configureButtonBindings() {
+    private void configureSwerveDriveCommands() {
         controller.getLeftStick()
-            .whileTrue(new RunCommand(
-                () -> swerveDrive.lockPosition(),
-                swerveDrive));
+            .whileTrue(
+                new RunCommand(
+                    () -> swerveDrive.lockPosition(),
+
+                    swerveDrive
+                )
+            );
 
         controller.getRightStick()
-            .whileTrue(new RunCommand(
-                () -> swerveDrive.zeroHeading(),
-                swerveDrive));
+            .whileTrue(
+                new RunCommand(
+                    () -> swerveDrive.zeroHeading(),
 
+                    swerveDrive
+                )
+            );
 
         controller.getA().whileTrue(followAprilTag);
     }
 
+    private void configureSuperCommands() {
+
+    }
+
     public Command getAutonomousCommand() {
         return autoSelector.getSelected();
     }

src/main/java/frc/robot/auto/PhotonRunnable.java

@@ -37,6 +37,7 @@ public PhotonRunnable(PhotonCamera camera) {
 
             // PV estimates will always be blue, they'll get flipped by robot thread
             layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+
             if(photonCamera != null) {
                 photonPoseEstimator = new PhotonPoseEstimator(
                     layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, photonCamera, APRILTAG_CAMERA_TO_ROBOT.inverse());

src/main/java/frc/robot/auto/commands/AmpAlign.java

@@ -0,0 +1,35 @@
+package frc.robot.auto.commands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.swerve.SwerveDrive;
+import java.util.function.Supplier;
+
+public class AmpAlign extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Supplier<Pose2d> poseProvider;
+
+    public AmpAlign(SwerveDrive swerveDrive, Supplier<Pose2d> poseProvider) {
+        this.swerveDrive = swerveDrive;
+        this.poseProvider = poseProvider;
+
+        addRequirements(swerveDrive);
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}