overloaded SpinUp constructor; added LongShot command on operator X b…

…utton press

src/main/java/frc/robot/Constants.java

@@ -51,6 +51,9 @@ public class Arm {
         // PID Profile constants
         public static final double MAXIMUM_VELOCITY = 150; // degrees per second
         public static final double MAXIMUM_ACCELERATION = 750; // degrees per second squared
+
+    		public static final double LONG_SHOT_ANGLE = 94;
+    		public static final double LONG_SHOT_SPEED = 8000;
     }
 
     public class PathPlanner {

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

@@ -17,6 +17,7 @@
 import frc.robot.auto.generator.PathPlannerAutoGenerator;
 import frc.robot.auto.generator.SimpleAutoGenerator;
 import frc.robot.cmdGroup.IntakeNote;
+import frc.robot.cmdGroup.LongShot;
 import frc.robot.drive.DefaultDrive;
 import frc.robot.drive.DriveSubsystem;
 import frc.robot.notification.LEDSubsystem;
@@ -36,9 +37,9 @@ public class RobotContainer {
   private final UndertakerSubsystem undertakerSubsystem;
   private final VisionSubsystem visionSubsystem;
   private final ArmSubsystem arm;
-  
+
   private final AbstractAutoGenerator autoGenerator;
-  
+
   private final RobotContext robotContext;
 
   public RobotContainer() {
@@ -57,13 +58,10 @@ public RobotContainer() {
 
     robotContext = new RobotContext(arm);
 
-    autoGenerator = 
-      Constants.PathPlanner.PATH_PLANNER_IS_ENABLED 
-      ? 
-      new PathPlannerAutoGenerator(drive, arm, shooter, undertakerSubsystem, robotContext) 
-      :
-      new SimpleAutoGenerator(drive, shooter, undertakerSubsystem, robotContext);
-
+    autoGenerator = Constants.PathPlanner.PATH_PLANNER_IS_ENABLED
+
+        ? new PathPlannerAutoGenerator(drive, arm, shooter, undertakerSubsystem, robotContext)
+        : new SimpleAutoGenerator(drive, shooter, undertakerSubsystem, robotContext);
 
     configureBindings();
     VersionFile.getInstance().putToDashboard();
@@ -78,9 +76,7 @@ private void configureBindings() {
             () -> getJoystickInput(driverController, Constants.Controller.LEFT_Y_AXIS),
             () -> getJoystickInput(driverController, Constants.Controller.LEFT_X_AXIS),
             () -> getJoystickInput(driverController, Constants.Controller.RIGHT_X_AXIS),
-            robotContext::getDriveSpeedScale
-            )
-    );
+            robotContext::getDriveSpeedScale));
 
     shooter.setDefaultCommand(new IntakeNote(shooter, undertakerSubsystem, robotContext::getReadyToIntake));
 
@@ -90,6 +86,8 @@ private void configureBindings() {
     operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION));
     operatorController.rightTrigger().onTrue(new MoveToHome(arm));
 
+    operatorController.x().onTrue(new LongShot(shooter, arm));
+
     operatorController.a().onTrue(new InstantCommand(() -> robotContext.setIntakeEnabledOverride(true)));
     operatorController.y().onTrue(new InstantCommand(() -> robotContext.setIntakeEnabledOverride(false)));
   }

src/main/java/frc/robot/arm/commands/MoveToPosition.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.arm.ArmSubsystem;
+
 public class MoveToPosition extends Command {
   private ArmSubsystem arm;
   private double targetAngle;
@@ -30,7 +31,7 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return debouncer.calculate (arm.isOnTarget());
+    return debouncer.calculate(arm.isOnTarget());
     }
   }
 

src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java

@@ -86,7 +86,7 @@ private void setStartingPose(String cmdName) {
     private void registerCommands() {
         NamedCommands.registerCommand("armHome", new MoveToHome(arm));
         NamedCommands.registerCommand("armAmp", new MoveToPosition(arm, Constants.Arm.AMP_POSITION));
-        NamedCommands.registerCommand("spinUp", new SpinUp(shooter, robotContext::shouldSlowShoot));
+        NamedCommands.registerCommand("spinUp", new SpinUp(shooter, Constants.Shooter.SHOOT_SPEED, robotContext::shouldSlowShoot));
         NamedCommands.registerCommand("shoot", new Shoot(shooter).withTimeout(Constants.Shooter.SHOOTER_SHOOT_TIME));
         NamedCommands.registerCommand("spinDown", new SpinDown(shooter));
         NamedCommands.registerCommand("runUndertaker", new UndertakerIntake(undertaker, robotContext::getReadyToIntake));

src/main/java/frc/robot/cmdGroup/LongShot.java

@@ -0,0 +1,18 @@
+package frc.robot.cmdGroup;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.arm.ArmSubsystem;
+import frc.robot.arm.commands.MoveToPosition;
+import frc.robot.shooter.ShooterSubsystem;
+import frc.robot.shooter.commands.Shoot;
+import frc.robot.shooter.commands.SpinUp;
+
+public class LongShot extends SequentialCommandGroup {
+  public LongShot(ShooterSubsystem shooter, ArmSubsystem arm) {
+    addCommands(
+        new MoveToPosition(arm, Constants.Arm.LONG_SHOT_ANGLE),
+        new SpinUp(shooter, Constants.Arm.LONG_SHOT_SPEED),
+        new Shoot(shooter).withTimeout(Constants.Shooter.SHOOTER_SHOOT_TIME));
+  }
+}

src/main/java/frc/robot/cmdGroup/SpinUpAndShoot.java

@@ -10,7 +10,7 @@
 public class SpinUpAndShoot extends SequentialCommandGroup {
   public SpinUpAndShoot(ShooterSubsystem shooter, BooleanSupplier slowShoot) {
     addCommands(
-        new SpinUp(shooter, slowShoot),
+        new SpinUp(shooter, Constants.Shooter.SHOOT_SPEED, slowShoot),
         new Shoot(shooter).withTimeout(Constants.Shooter.SHOOTER_SHOOT_TIME));
   }
 }

src/main/java/frc/robot/shooter/commands/SpinUp.java

@@ -9,17 +9,26 @@
 public class SpinUp extends Command {
   private final ShooterSubsystem subsystem;
   private final BooleanSupplier slowShoot;
+  private final double targetSpeed;
 
-  public SpinUp(ShooterSubsystem subsystem, BooleanSupplier slowShoot) {
+  public SpinUp(ShooterSubsystem subsystem, double targetSpeed, BooleanSupplier slowShoot) {
     this.subsystem = subsystem;
     this.slowShoot = slowShoot;
+    this.targetSpeed = targetSpeed;
+    addRequirements(subsystem);
+  }
+
+  public SpinUp(ShooterSubsystem subsystem, double targetSpeed) {
+    this.subsystem = subsystem;
+    this.slowShoot = () -> false;
+    this.targetSpeed = targetSpeed;
     addRequirements(subsystem);
   }
 
   @Override
   public void initialize() {
     double scale = slowShoot.getAsBoolean() ? Constants.Shooter.AMP_SHOOT_SCALE : 1;
-    subsystem.setTargetShooterSpeed(Constants.Shooter.SHOOT_SPEED * scale);
+    subsystem.setTargetShooterSpeed(targetSpeed * scale);
   }
 
   @Override