LongShot Command (#73)

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

@@ -51,6 +51,8 @@ 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 = 105;
     }
 
     public class PathPlanner {
@@ -117,6 +119,7 @@ public class Shooter {
         public static final double INDEXER_MOI = 1;
 
         public static final double AMP_SHOOT_SCALE = 0.4;
+        public static final double LONG_SHOT_SPEED = 5000;
     }
 
     public class Dashboard {

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.PodiumShot;
 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() {
@@ -58,12 +59,11 @@ public RobotContainer() {
     robotContext = new RobotContext(arm);
 
     autoGenerator = 
-      Constants.PathPlanner.PATH_PLANNER_IS_ENABLED 
-      ? 
-      new PathPlannerAutoGenerator(drive, arm, shooter, undertakerSubsystem, robotContext) 
+      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 +78,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 +88,8 @@ private void configureBindings() {
     operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION));
     operatorController.rightTrigger().onTrue(new MoveToHome(arm));
 
+    operatorController.x().onTrue(new PodiumShot(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, Constants.Shooter.SHOOT_SPEED * 0.9, 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/PodiumShot.java

@@ -0,0 +1,19 @@
+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 PodiumShot extends SequentialCommandGroup {
+  public PodiumShot(ShooterSubsystem shooter, ArmSubsystem arm) {
+    addCommands(
+        new MoveToPosition(arm, Constants.Arm.LONG_SHOT_ANGLE),
+        new SpinUp(shooter, Constants.Shooter.LONG_SHOT_SPEED, Constants.Shooter.LONG_SHOT_SPEED),
+        new Shoot(shooter).withTimeout(Constants.Shooter.SHOOTER_SHOOT_TIME),
+        new MoveToPosition(arm, Constants.Arm.HOME_POSITION));
+  }
+}

src/main/java/frc/robot/shooter/ShooterSubsystem.java

@@ -5,14 +5,17 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import frc.robot.Constants;
 import frc.robot.Robot;
 
 public class ShooterSubsystem extends SubsystemBase {
   private IShooterIO shooterIO;
-  private double targetShooterSpeed = 0;
+  private double targetTopShooterSpeed = 0;
+  private double targetBottomShooterSpeed = 0;
   private double targetIndexerSpeed = 0;
+
+  private double topOutput = 0;
+  private double bottomOutput = 0;
 
   private PIDController topPidController = new PIDController(Constants.Shooter.SHOOTER_PID_P,
       Constants.Shooter.SHOOTER_PID_I, Constants.Shooter.SHOOTER_PID_D);
@@ -36,7 +39,8 @@ public ShooterSubsystem(IShooterIO shooterIO) {
 
   private void ShuffleboardInit() {
     ShuffleboardTab tab = Shuffleboard.getTab(Constants.Shooter.SHUFFLEBOARD_TAB);
-    tab.addDouble("Target Shooter Speed", () -> targetShooterSpeed).withPosition(0, 0).withSize(2, 1);
+    tab.addDouble("Target Top Shooter Speed", () -> targetTopShooterSpeed).withPosition(0, 0).withSize(2, 1);
+    tab.addDouble("Target Bottom Shooter Speed", () -> targetBottomShooterSpeed).withPosition(1, 0).withSize(2, 1);
     tab.addDouble("Shooter 1 Speed", () -> shooterIO.getShooterSpeeds()[0]).withPosition(2, 0);
     tab.addDouble("Shooter 2 Speed", () -> shooterIO.getShooterSpeeds()[1]).withPosition(3, 0);
     tab.addDouble("Shooter Speed Diff.", () -> shooterIO.getShooterSpeeds()[0] - shooterIO.getShooterSpeeds()[1])
@@ -51,6 +55,8 @@ private void ShuffleboardInit() {
     tab.addDouble("Indexer Speed Diff.", () -> shooterIO.getIndexerSpeeds()[0] - shooterIO.getIndexerSpeeds()[1])
         .withPosition(4, 1).withSize(2, 1);
     tab.addDouble("LaserCAN Measurement", () -> shooterIO.getMeasurement());
+    tab.addDouble("Top Output", () -> topOutput);
+    tab.addDouble("Bottom Output", () -> bottomOutput);
   }
 
   public boolean isIndexerLoaded() {
@@ -59,12 +65,13 @@ public boolean isIndexerLoaded() {
 
   public boolean isShooterReady() {
     double[] speeds = shooterIO.getShooterSpeeds();
-    return Math.abs(targetShooterSpeed - speeds[0]) < Constants.Shooter.TARGET_SPEED_ERROR_MARGIN
-        && Math.abs(targetShooterSpeed * 0.9 - speeds[1]) < Constants.Shooter.TARGET_SPEED_ERROR_MARGIN;
+    return Math.abs(targetTopShooterSpeed - speeds[0]) < Constants.Shooter.TARGET_SPEED_ERROR_MARGIN
+        && Math.abs(targetBottomShooterSpeed - speeds[1]) < Constants.Shooter.TARGET_SPEED_ERROR_MARGIN;
   }
 
-  public void setTargetShooterSpeed(double targetSpeed) {
-    targetShooterSpeed = targetSpeed;
+  public void setTargetShooterSpeeds(double targetTopSpeed, double targetBottomSpeed) {
+    targetTopShooterSpeed = targetTopSpeed;
+    targetBottomShooterSpeed = targetBottomSpeed;
   }
 
   public void setIndexerSpeed(double targetSpeed) {
@@ -74,11 +81,11 @@ public void setIndexerSpeed(double targetSpeed) {
 
   @Override
   public void periodic() {
-    shooterIO.setShooterOutput(
-        topPidController.calculate(shooterIO.getShooterSpeeds()[0], targetShooterSpeed)
-            + topFeedForward.calculate(targetShooterSpeed),
-        bottomPidController.calculate(shooterIO.getShooterSpeeds()[1], targetShooterSpeed * 0.9)
-            + bottomFeedForward.calculate(targetShooterSpeed * 0.9));
+    topOutput = topPidController.calculate(shooterIO.getShooterSpeeds()[0], targetTopShooterSpeed)
+        + topFeedForward.calculate(targetTopShooterSpeed);
+    bottomOutput = bottomPidController.calculate(shooterIO.getShooterSpeeds()[1], targetBottomShooterSpeed)
+        + bottomFeedForward.calculate(targetBottomShooterSpeed);
+    shooterIO.setShooterOutput(topOutput, bottomOutput);
 
     shooterIO.periodic();
   }

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

@@ -19,7 +19,7 @@ public void initialize() {
   @Override
   public void end(boolean interrupted) {
     subsystem.setIndexerSpeed(0);
-    subsystem.setTargetShooterSpeed(0);
+    subsystem.setTargetShooterSpeeds(0, 0);
   }
 
   @Override

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

@@ -16,7 +16,7 @@ public SpinDown(ShooterSubsystem subsystem) {
 
   @Override
   public void initialize() {
-    subsystem.setTargetShooterSpeed(0);
+    subsystem.setTargetShooterSpeeds(0, 0);
   }
 
   @Override

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

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

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

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