diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0225ec1..ffc78af 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -188,7 +188,9 @@ public static final class ShooterConstants{ public static final double shootingRMPAutoLine = 3000; - public static final double shooterRPMThreshold = 50; + public static final double shooterRPMThreshold = 100; + + public static final double shooterAutoRPMThreshold = 2000; } public static final class IntakeConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9368c91..33ecfeb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import frc.robot.Constants.OIConstants; +import frc.robot.commands.AutoCommmands; import frc.robot.commands.ComboCommands; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.Elevator; @@ -50,6 +51,8 @@ public class RobotContainer { //combination commands class ComboCommands comboCommands = new ComboCommands(elevator, intake, transfer, shooter); + AutoCommmands autoCommmands = new AutoCommmands(robotDrive, intake, elevator, transfer, shooter, comboCommands); + // Auton selector for dashboard SendableChooser autoSelector = new SendableChooser<>(); @@ -249,7 +252,7 @@ public Command getAutonomousCommand() { * https://github.com/mjansen4857/pathplanner */ - return new PathPlannerAuto("SegmentedTopFour"); + return autoCommmands.shootOnStart(); } /*◇─◇──◇─◇ diff --git a/src/main/java/frc/robot/commands/AutoCommmands.java b/src/main/java/frc/robot/commands/AutoCommmands.java index ad6c2a1..fe16c2e 100644 --- a/src/main/java/frc/robot/commands/AutoCommmands.java +++ b/src/main/java/frc/robot/commands/AutoCommmands.java @@ -1,6 +1,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.Elevator; @@ -28,16 +29,19 @@ public AutoCommmands(DriveSubsystem robotDrive, Intake intake, Elevator elevator public Command shootOnStart() { return new SequentialCommandGroup( - shooter.shooterAutoLineRevUpCommand(), - new StepCommand(transfer.shootCommand(), shooter::bothAtSetpointRPM, transfer) + new ParallelCommandGroup( + shooter.shooterAutoLineRevUpCommand(), + new StepCommand(transfer.shootCommand(), shooter::bothAtSetpointRPM, transfer) + ).until(transfer::shooterNotFull), + shooter.stopShooterCommand() ); } - // public Command autoIntake() { - // return new SequentialCommandGroup( - // combo.startShooterIntakeCommand() - // .until(transfer::shooterFull) - // .andThen(combo.stowCommandGroup()) - // ); - // } + public Command autoIntakeToShooter() { + return new SequentialCommandGroup( + combo.startShooterIntakeCommand() + .until(transfer::shooterFull) + .andThen(combo.stowCommandGroup()) + ); + } } diff --git a/src/main/java/frc/robot/commands/StepCommand.java b/src/main/java/frc/robot/commands/StepCommand.java index 4102454..1bd8cb4 100644 --- a/src/main/java/frc/robot/commands/StepCommand.java +++ b/src/main/java/frc/robot/commands/StepCommand.java @@ -24,6 +24,7 @@ public class StepCommand extends Command { */ public StepCommand(Command toRun, BooleanSupplier condition, Subsystem... requirements) { this.toRun = toRun; + this.condition = condition; addRequirements(requirements); } @@ -41,6 +42,7 @@ public void execute() { toRun.schedule(); hasRun = true; } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index bb4cb81..85701cb 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -135,12 +135,14 @@ public double getBottomWheelRPM() { @Log public boolean topAtSetpointRPM() { - return Math.abs(getTopWheelRPM() - shooterTopSetpoint) < ShooterConstants.shooterRPMThreshold; + //return Math.abs(getTopWheelRPM() - shooterTopSetpoint) < ShooterConstants.shooterRPMThreshold; + return getTopWheelRPM() > 2000; } @Log public boolean bottomAtSetpointRPM() { - return Math.abs(getBottomWheelRPM() - shooterBottomSetpoint) < ShooterConstants.shooterRPMThreshold; + //return Math.abs(getBottomWheelRPM() - shooterBottomSetpoint) < ShooterConstants.shooterRPMThreshold; + return getTopWheelRPM() > 2000; } @Log public boolean bothAtSetpointRPM() {