diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4e50c9fb..38e88a91 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -130,6 +130,7 @@ public class RobotContainer { DoubleSupplier zeroSup; BooleanSupplier intakeReverse; + Command autoPickup; // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -287,10 +288,17 @@ private void configureBindings() { )); if(Preferences.getBoolean("Detector Limelight", false)) { - new Trigger(operatorController::getR1Button).whileTrue(new SequentialCommandGroup( - new CollectNote(driveTrain, limelight), - new DriveTimeCommand(-1, 0, 0, 1, driveTrain) - )); + autoPickup = new ParallelRaceGroup( + new AutoGroundIntake(indexer, intake, angleShooterSubsystem), + new SequentialCommandGroup( + new CollectNote(driveTrain, limelight), + new DriveTimeCommand(-1, 0, 0, 1.5, driveTrain), + new DriveTimeCommand(1, 0, 0, 0.5, driveTrain), + new DriveTimeCommand(-1, 0, 0, 0.5, driveTrain), + new WaitCommand(0.5) + ) + ).withTimeout(4); + new Trigger(operatorController::getR1Button).whileTrue(autoPickup); } new Trigger(ForceVisionSup).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false))); SmartDashboard.putData("force update limelight position", new InstantCommand(()->driveTrain.forceUpdateOdometryWithVision(), driveTrain)); @@ -445,15 +453,7 @@ private void registerNamedCommands() { new AutoGroundIntake(indexer, intake, angleShooterSubsystem) )); if(intakeExists&&indexerExists&&angleShooterExists) { - NamedCommands.registerCommand("autoPickup",new ParallelCommandGroup( - new AutoGroundIntake(indexer, intake, angleShooterSubsystem), - new SequentialCommandGroup( - new CollectNote(driveTrain, limelight), - new DriveTimeCommand(-1, 0, 0, 1.5, driveTrain), - new DriveTimeCommand(1, 0, 0, 0.5, driveTrain) - ) - ).withTimeout(3.5) - ); + NamedCommands.registerCommand("autoPickup", autoPickup); } if(intakeExists&&!indexerExists&&!angleShooterExists) { NamedCommands.registerCommand("groundIntake", new InstantCommand(()->intake.intakeYes(IntakeConstants.INTAKE_SPEED))); diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 2f40d3ee..7d1b2696 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -265,7 +265,7 @@ public static final class ShooterConstants{ public static final double HUMAN_PLAYER_ANGLE = 97; public static final double HUMAN_PLAYER_RPS = -15; public static final double SAFE_SHOOTER_ANGLE = 15; - public static final double GROUND_INTAKE_SHOOTER_ANGLE = 90; + public static final double GROUND_INTAKE_SHOOTER_ANGLE = 69; /** * the values used when adjusting the shooter's angle based on our speaker distance. Here's how we calculated them: *
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 5b553a46..f311faaf 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -42,8 +42,7 @@ public IntakeSubsystem() { } else { m_DistanceSensor = intake2.getAnalog(Mode.kAbsolute); } - intake2.follow(intake1); - intake2.setInverted(false); + intake2.setInverted(true); intake1.setInverted(true); intake1.setIdleMode(IdleMode.kCoast); intake2.setIdleMode(IdleMode.kCoast); @@ -65,6 +64,7 @@ public IntakeSubsystem() { */ public void intakeYes(double intakeRunSpeed) { intake1.set(intakeRunSpeed); + intake2.set(intakeRunSpeed); } /** * sets the intakes speed @@ -73,12 +73,14 @@ public void intakeYes(double intakeRunSpeed) { */ public void intakeNo(double intakeRunSpeed) { intake1.set(-intakeRunSpeed); + intake2.set(-intakeRunSpeed); } /** * sets the intake's power to 0 */ public void intakeOff() { intake1.set(0); + intake2.set(0); } /** * uses the distance sensor inside the indexer to tell if there is a note fully inside the indexer