diff --git a/CompSeasonBot2021/src/main/java/frc/robot/Constants.java b/CompSeasonBot2021/src/main/java/frc/robot/Constants.java index 37db066..b482c7a 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/Constants.java +++ b/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; } diff --git a/CompSeasonBot2021/src/main/java/frc/robot/RobotContainer.java b/CompSeasonBot2021/src/main/java/frc/robot/RobotContainer.java index aebfa9a..ae56669 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/RobotContainer.java +++ b/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); diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/AutoSequentialShooter.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/AutoSequentialShooter.java index f76e200..a07de7d 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/AutoSequentialShooter.java +++ b/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)); } } \ No newline at end of file diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/FINiteRechargeAutoPath.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/FINiteRechargeAutoPath.java new file mode 100644 index 0000000..d276988 --- /dev/null +++ b/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)); + } +} diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightShooter.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightShooter.java index 56fb3a8..7b4af66 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightShooter.java +++ b/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)); } } diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/SequentialShooter.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/SequentialShooter.java index 6dc7006..929d776 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/SequentialShooter.java +++ b/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"); } diff --git a/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Intake.java b/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Intake.java index 68ced49..4453542 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Intake.java +++ b/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 diff --git a/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Shooter.java b/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Shooter.java index 348fe00..f3e0606 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Shooter.java +++ b/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