slide and shot auto

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

@@ -25,6 +25,7 @@
 import frc.robot.commands.ZeroHeadingCommand;
 import frc.robot.commands.auto.DriveAutoCommand;
 import frc.robot.commands.auto.ShootAndDriveAutoCommand;
+import frc.robot.commands.auto.SlideAndShootAutoCommand;
 import frc.robot.commands.auto.TurnAndShootAutoCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
@@ -93,6 +94,12 @@ public RobotContainer() {
       new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive back only",
       new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto
+    m_chosenAuto.addOption("Shoot only", 
+      new ShootCommand(m_shooterSubsystem));
+    m_chosenAuto.addOption("Slide, shoot, slide back, and drive from left", 
+      new SlideAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 0));
+    m_chosenAuto.addOption("Slide, shoot, slide back, and drive from right", 
+      new SlideAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 1));
 
     m_tab.add(m_chosenAuto);
 

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

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.auto;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.SetPoseCommand;
+import frc.robot.commands.ShootCommand;
+import frc.robot.commands.SwerveDriveCommand;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class SlideAndShootAutoCommand extends SequentialCommandGroup {
+  public SlideAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, int direction) {
+    super(
+        new WaitCommand(4),
+      new SetPoseCommand(swerve, startingOffset).withTimeout(0),
+      new SwerveDriveCommand(swerve, supplyDouble(direction == 0 ? -.3 : .3), supplyDouble(0), supplyDouble(0), true, supplyBoolean(true), supplyDouble(0))
+      .withTimeout(1),
+      new ShootCommand(shoot),
+      new SwerveDriveCommand(swerve, supplyDouble(direction == 0 ? .3 : -.3), supplyDouble(0), supplyDouble(0), true, supplyBoolean(true), supplyDouble(0))
+      .withTimeout(1),
+      new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyDouble(0))
+    );
+  }
+
+  private static Supplier<Double> supplyDouble(double d) {return new Supplier<Double>() {@Override public Double get() {return d;}};}
+  private static Supplier<Boolean> supplyBoolean(boolean b) {return new Supplier<Boolean>() {@Override public Boolean get() {return b;}};}
+}