Added Shoot

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

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.commands.ExampleCommand;
+import frc.robot.commands.Shoot;
 import frc.robot.commands.Drivetrain.ArcadeDrive;
 import frc.robot.commands.Drivetrain.CharacterizeDrivetrain;
 import frc.robot.commands.Flywheel.CharacterizeFlywheel;
@@ -25,6 +26,7 @@
 import frc.robot.commands.Lift.LiftToHeight;
 import frc.robot.commands.Lift.LiftUp;
 import frc.robot.subsystems.DifferentialDrivetrain;
+import frc.robot.subsystems.Feeder;
 import frc.robot.subsystems.Flywheel;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Lift;
@@ -58,6 +60,7 @@ public class RobotContainer {
   private final Lift m_lift = new Lift();
   private final Intake m_intake = new Intake();
   private final Flywheel m_flywheel = new Flywheel();
+  private final Feeder m_feeder = new Feeder();
 
   private final ExampleCommand m_autoCommand = new ExampleCommand();
 
@@ -92,8 +95,8 @@ private void configureButtonBindings() {
 //
     //new JoystickButton(m_buttonbox, 7/* button label */).whileHeld(new IntakeSuck(m_intake));
     //new JoystickButton(m_buttonbox, 8/* button label */).whileHeld(new IntakeSpit(m_intake));
-    new JoystickButton(m_buttonbox, 7).whenPressed(new FlywheelToSpeed(m_flywheel, 80));
-    new JoystickButton(m_buttonbox, 8).whenPressed(new FlywheelToSpeed(m_flywheel, 0));
+    new JoystickButton(m_buttonbox, 7).whileHeld(new Shoot(m_flywheel, m_feeder)).whenReleased(new FlywheelToSpeed(m_flywheel, 0));
+    //new JoystickButton(m_buttonbox, 8).whenPressed(new FlywheelToSpeed(m_flywheel, 0));
   }
 
   /**

src/main/java/frc/robot/commands/Feeder/FeedAmmo.java

@@ -0,0 +1,42 @@
+// 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.Feeder;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Feeder;
+
+public class FeedAmmo extends CommandBase {
+  Feeder m_feeder;
+  /** Creates a new FeedAmmo. */
+  public FeedAmmo(Feeder feeder) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(feeder);
+    m_feeder = feeder;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_feeder.enableFeeder();
+    m_feeder.enableKicker();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_feeder.disableFeeder();
+    m_feeder.disableKicker();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Flywheel/FlywheelMaintainSpeed.java

@@ -0,0 +1,41 @@
+// 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.Flywheel;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Flywheel;
+
+public class FlywheelMaintainSpeed extends CommandBase {
+  Flywheel m_flywheel;
+  double m_speed;
+  /** Creates a new FlywheelMaintainSpeed. */
+  public FlywheelMaintainSpeed(Flywheel flywheel) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(flywheel);
+    m_flywheel = flywheel;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_speed = m_flywheel.getVelocity();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_flywheel.setVelocity(m_speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return Math.abs(m_flywheel.getVelocity() - m_speed) > 10;
+  }
+}

src/main/java/frc/robot/commands/Flywheel/FlywheelToSpeed.java

@@ -11,7 +11,7 @@ public class FlywheelToSpeed extends CommandBase {
   Flywheel m_flywheel;
   double m_speed;
   /** Creates a new FlywheelToSpeed. */
-  public FlywheelToSpeed(Flywheel flywheel,double speed) {
+  public FlywheelToSpeed(Flywheel flywheel, double speed) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(flywheel);
     m_flywheel = flywheel;
@@ -36,6 +36,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return Math.abs(m_flywheel.getVelocity() - m_speed) < 5;
   }
 }

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

@@ -0,0 +1,30 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.Feeder.FeedAmmo;
+import frc.robot.commands.Feeder.PrepareAmmo;
+import frc.robot.commands.Flywheel.FlywheelMaintainSpeed;
+import frc.robot.commands.Flywheel.FlywheelToSpeed;
+import frc.robot.subsystems.Feeder;
+import frc.robot.subsystems.Flywheel;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class Shoot extends SequentialCommandGroup {
+  /** Creates a new Shoot. */
+  public Shoot(Flywheel flywheel, Feeder feeder) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+      new ParallelRaceGroup(new FlywheelToSpeed(flywheel, 80), new PrepareAmmo(feeder)),
+      new ParallelRaceGroup(new FlywheelMaintainSpeed(flywheel), new FeedAmmo(feeder))
+    );
+  }
+}