Pre FINite Recharge

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

@@ -25,7 +25,7 @@ public final class Constants {
 
     public static final class MechConstants {
         public static final double kBFSpeed = .75;
-        public static final double kSFSpeed = 1;
+        public static final double kSFSpeed = -1;
     }
 
 

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

@@ -27,6 +27,7 @@
 import frc.robot.commands.auto.SequentialShooter;
 import frc.robot.commands.auto.SlalomPathArc;
 import frc.robot.commands.auto.BouncePathArc;
+import frc.robot.commands.auto.FINiteRechargeAutoPath;
 import frc.robot.commands.auto.BarrelPathArc;
 import frc.robot.oi.CONSTANTS_OI;
 import frc.robot.oi.Logitech;
@@ -65,10 +66,11 @@ public class RobotContainer {
   private final Command m_BounceAuto = new BouncePathArc(drivetrain);
   // private final Command m_autoCommand = new LimelightCenter(drivetrain);
   private final Command m_FailSafeBackward = new FailSafeAutoBackward(drivetrain, shooter, ballFeed);
-  private final Command m_SequentialShooter = new AutoSequentialShooter(shooter, ballFeed);
+  private final Command m_SequentialShooter = new AutoSequentialShooter(shooter, ballFeed, 4900);
   private final Command m_ShootThenBackup = new SequentialCommandGroup(
-      new AutoSequentialShooter(shooter, ballFeed).withTimeout(10),
+      new AutoSequentialShooter(shooter, ballFeed, 4900).withTimeout(10),
       new AutoForwardMotionMagic(drivetrain, -36));
+  private final Command m_FINiteRechargeCommand = new FINiteRechargeAutoPath(limelight, drivetrain, shooter, ballFeed, hood);
   private final Command m_GalacticAuto = new GalacticSearch(drivetrain, intake, pixy, ballFeed);
 
   //Create a chooser for auto
@@ -174,7 +176,8 @@ private void configureButtonBindings() {
 
     //Run all feeds in reverse while held.  Stop all feeds when released
     //Allows operator to clear jams or release balls
-    logitecButtonA.whenHeld(new RunIntakeAndBallFeedAndShooterFeed(intake, ballFeed, -.35, -.75, -1.0),false);  //Speeds as previously determined
+    logitecButtonA.whenHeld(new RunIntakeAndBallFeedAndShooterFeed(intake, ballFeed, -.35,
+        -MechConstants.kBFSpeed, -MechConstants.kSFSpeed),false);  //Speeds as previously determined
 
     //Run shooter wheel at selected speed. Continues until cancelled by B button
     //Changed these from toggle as with a toggle it is difficult for the operator to know what state it is in.
@@ -295,6 +298,8 @@ private void configureDashboard(){
     m_AutoChooser.addOption("OLD - AutoSequentialShooter", m_SequentialShooter);  //Don't know if this works
     m_AutoChooser.addOption("OLD - FailSafeAutoBackwards", m_FailSafeBackward);  //Don't know if this works
     m_AutoChooser.addOption("NEW - ShootTenBackup", m_ShootThenBackup);
+    m_AutoChooser.addOption("FINiteRecharge", m_FINiteRechargeCommand);
+
 
     /* Removing so we don't accidently select during competition.
     m_AutoChooser.addOption("Slalom", m_SlalomAuto);

CompSeasonBot2021/src/main/java/frc/robot/commands/auto/AutoSequentialShooter.java

@@ -19,9 +19,9 @@ public class AutoSequentialShooter extends ParallelRaceGroup {
   /**
    * Creates a new AutoSequentialShooter.
    */
-  public AutoSequentialShooter(Shooter shooter, BallFeed ballFeed) {
+  public AutoSequentialShooter(Shooter shooter, BallFeed ballFeed, double RPM) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());super();
-    super(new RunShooterPID(shooter, 4900), new SequentialShooter(shooter, ballFeed));
+    super(new RunShooterPID(shooter, RPM), new SequentialShooter(shooter, ballFeed));
   }
 }

CompSeasonBot2021/src/main/java/frc/robot/commands/auto/FINiteRechargeAutoPath.java

@@ -0,0 +1,24 @@
+// 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 edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.ExtendHood;
+import frc.robot.subsystems.*;
+
+// 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 FINiteRechargeAutoPath extends SequentialCommandGroup {
+  /** Creates a new FINiteRechargeAutoPath. */
+  public FINiteRechargeAutoPath(Limelight limelight, Drivetrain drivetrain, Shooter shooter, BallFeed ballfeed, Hood hood) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(new ExtendHood(hood),
+                new LimelightCenter(limelight, drivetrain),
+                new AutoSequentialShooter(shooter, ballfeed, 3400).withTimeout(8),
+                new AutoForwardMotionMagic(drivetrain,36));
+  }
+}

CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightShooter.java

@@ -20,6 +20,6 @@ public class LimelightShooter extends SequentialCommandGroup {
   public LimelightShooter(Limelight limelight, Drivetrain drivetrain, Shooter shooter, BallFeed ballFeed) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
-    super(new LimelightCenter(limelight, drivetrain), new AutoSequentialShooter(shooter, ballFeed));
+    super(new LimelightCenter(limelight, drivetrain), new AutoSequentialShooter(shooter, ballFeed, 4900));
   }
 }

CompSeasonBot2021/src/main/java/frc/robot/commands/auto/SequentialShooter.java

@@ -1,6 +1,8 @@
 package frc.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import frc.robot.commands.RunBothFeeders;
 import frc.robot.subsystems.BallFeed;
@@ -16,12 +18,9 @@ public class SequentialShooter extends SequentialCommandGroup {
 
   public SequentialShooter(Shooter shooter, BallFeed ballFeed) {
      // Add your commands in the super() call, e.g.
-     super(new WaitUntilCommand(shooter::isStable),
-          new RunBothFeeders(ballFeed, MechConstants.kBFSpeed, MechConstants.kSFSpeed).withInterrupt(shooter::isNotStable),
-          new WaitUntilCommand(shooter::isStable),
-          new RunBothFeeders(ballFeed, MechConstants.kBFSpeed, MechConstants.kSFSpeed).withInterrupt(shooter::isNotStable),
-          new WaitUntilCommand(shooter::isStable),
-          new RunBothFeeders(ballFeed, MechConstants.kBFSpeed, MechConstants.kSFSpeed).withInterrupt(shooter::isNotStable)
+     super(/*new WaitUntilCommand(shooter::isStable)*/
+          new WaitCommand(3),
+          new RunBothFeeders(ballFeed, MechConstants.kBFSpeed, MechConstants.kSFSpeed)
           );
           //System.out.println("running SequentialShooter");
   }

CompSeasonBot2021/src/main/java/frc/robot/subsystems/Intake.java

@@ -44,10 +44,10 @@ public void stopIntake(){
     intakeMotor.set(0.0);
   }
   public void extendIntake(){
-    intakeSolenoid.set(false);
+    intakeSolenoid.set(true);
   }
   public void retractIntake(){
-     intakeSolenoid.set(true);
+     intakeSolenoid.set(false);
   }
 
   @Override

CompSeasonBot2021/src/main/java/frc/robot/subsystems/Shooter.java

@@ -121,7 +121,7 @@ public double getShooterSpeed() {
 
   // Created for compatibilty with existing code, may want to do this differently
   public boolean isStable(){
-    return(withinLoops > kStableLoops);
+    return(withinLoops > kStableLoops && getShooterSpeed() > 3000);
   }
 
   // Created for compatibility with existing code, may want to do this differently