Examples with WaitCommand edu.wpi.first.wpilibj2.command.WaitCommand used on opensource projects

Example 1 with WaitCommand

use of edu.wpi.first.wpilibj2.command.WaitCommand in project FRC-2022-First-Iteration by SkylineSpartabots.

the class AutonomousCommandFactory method blueFourBallAuto.

public static Command blueFourBallAuto() {
    DrivetrainSubsystem m_drivetrainSubsystem = DrivetrainSubsystem.getInstance();
    Pose2d startPose = getPose(7.72, 2.82, -110);
    Pose2d ball1 = getPose(5.36, 1.98, -160);
    Pose2d ball2 = getPose(1.13, 1.33, -137);
    Pose2d ball3 = getPose(7.63, 0.65, -90);
    // SET STARTING POSITION
    Command resetOdo = new InstantCommand(() -> m_drivetrainSubsystem.resetOdometryFromPosition(startPose), m_drivetrainSubsystem);
    Command turnOnIntake = new SetIntakeCommand(intakeOn);
    Command rampUpShooter = new SetShooterCommand(shooterRamp);
    Command driveToFirstBall = new TrajectoryDriveCommand(ball1, List.of(), false);
    Command driveBackToShoot = new TrajectoryDriveCommand(startPose, List.of(), true);
    Command fireIndexer = new SetIndexerCommand(indexerFire);
    Command waitForShooterToFinish = new WaitCommand(2);
    Command turnOffShooter = new SetShooterCommand(shooterOff);
    Command driveToSecondBall = new TrajectoryDriveCommand(ball2, List.of(), false);
    Command driveBackToShootSecondTime = new TrajectoryDriveCommand(startPose, List.of(), true);
    Command runIndexerDown = new SetIndexerCommand(indexerDown);
    Command waitForShooterToRamp = new WaitCommand(1);
    Command fireIndexer2 = new SetIndexerCommand(indexerFire);
    Command waitForShooterToFinish2 = new WaitCommand(2);
    Command turnOffIndexer2 = new SetIndexerCommand(indexerOff);
    Command turnOffShooter2 = new SetShooterCommand(shooterOff);
    Command driveToThirdBall = new TrajectoryDriveCommand(ball3, List.of(), false);
    Command turnIntakeOff = new SetIntakeCommand(intakeOff);
    return new SequentialCommandGroup(resetOdo, turnOnIntake, rampUpShooter, driveToFirstBall, driveBackToShoot, fireIndexer, waitForShooterToFinish, turnOffShooter, driveToSecondBall, driveBackToShootSecondTime, runIndexerDown, waitForShooterToRamp, fireIndexer2, waitForShooterToFinish2, turnOffIndexer2, turnOffShooter2, driveToThirdBall, turnIntakeOff);
}

Example 2 with WaitCommand

use of edu.wpi.first.wpilibj2.command.WaitCommand in project RapidReact2022-340 by Greater-Rochester-Robotics.

the class RobotContainer method configureAutoModes.

/**
 * Define all autonomous modes here to have them
 * appear in the autonomous select drop down menu.
 * They will appear in the order entered
 */
private void configureAutoModes() {
    autoChooser.setDefaultOption("Wait 1 sec(do nothing)", new WaitCommand(1));
    autoChooser.addOption("Drives backwards 1.5 robot, shoots", new AutoDriveBackAndShoot(1.5));
    autoChooser.addOption("Drives backwards 2.5 robot, shoots", new AutoDriveBackAndShoot(2.5));
    autoChooser.addOption("Grab left ball, shoot", new AutoLeftTwoBall());
    autoChooser.addOption("Grab mid ball, shoot", new AutoMidTwoBall());
    autoChooser.addOption("Grab right ball, shoot", new AutoRightTwoBall());
    autoChooser.addOption("Shoot, grab right then mid ball, shoot", new AutoRightThreeBall());
    autoChooser.addOption("Grab mid ball, shoot. Get 2 terminal, shoot", new AutoMidFourBall());
    autoChooser.addOption("Partner ball, shoot, grab left, shoot", new AutoPartnerPickupLeftBall());
    // autoChooser.addOption("5 ball", new AutoRightFiveBall());
    SmartDashboard.putData(RobotContainer.autoChooser);
}

Example 3 with WaitCommand

use of edu.wpi.first.wpilibj2.command.WaitCommand in project 2022RobotCode by AusTINCANsProgrammingTeam.

the class AutonModes method initializeCommandGroups.

private void initializeCommandGroups() {
    taxiCommand = new SequentialCommandGroup(// deploy/extend the intake
    new DeployIntake(intakeSubsystem, cdsSubsystem), // wait before starting, units in seconds
    new WaitCommand(initialWaitTime), taxiRamseteCommand.andThen(() -> driveBaseSubsystem.stopDriveMotors()));
    if (shooterEnabled) {
        oneBallCommand = new SequentialCommandGroup(new DeployIntake(intakeSubsystem, cdsSubsystem), new WaitCommand(initialWaitTime), new ShooterPressed(shooterSubsystem, limelightSubsystem, cdsSubsystem, false), new WaitCommand(Constants.delaytaxi), oneBallRamseteCommand.andThen(() -> driveBaseSubsystem.stopDriveMotors()));
        twoBallParallel = new ParallelDeadlineGroup(twoBallRamseteCommands[0].andThen(// travel to get ball
        () -> driveBaseSubsystem.stopDriveMotors()), new IntakeForwardCommand(intakeSubsystem), new CDSForwardCommand(cdsSubsystem));
        twoBallCommand = new SequentialCommandGroup(new DeployIntake(intakeSubsystem, cdsSubsystem), new WaitCommand(initialWaitTime), twoBallParallel, twoBallRamseteCommands[1].andThen(() -> driveBaseSubsystem.stopDriveMotors()), new WaitCommand(Constants.delayshot), new ShooterPressed(shooterSubsystem, limelightSubsystem, cdsSubsystem, false));
        threeBallParallel = new ParallelDeadlineGroup(threeBallRamseteCommands[0].andThen(// travel to get the two balls
        () -> driveBaseSubsystem.stopDriveMotors()), new IntakeForwardCommand(intakeSubsystem), new CDSForwardCommand(cdsSubsystem));
        threeBallCommand = new SequentialCommandGroup(new DeployIntake(intakeSubsystem, cdsSubsystem), new WaitCommand(initialWaitTime), new ShooterPressed(shooterSubsystem, limelightSubsystem, cdsSubsystem, // shoot preloaded
        false), threeBallParallel, threeBallRamseteCommands[1].andThen(() -> driveBaseSubsystem.stopDriveMotors()), new ShooterPressed(shooterSubsystem, limelightSubsystem, cdsSubsystem, // shoot the two acquired balls
        false));
    // fourBallCommand
    // fiveBallCommand
    }
}

Example 4 with WaitCommand

use of edu.wpi.first.wpilibj2.command.WaitCommand in project CodeCloset2022 by FRC7540.

the class RobotContainer method autonomous.

public void autonomous() {
    Command script = new SequentialCommandGroup(// FOR FUTURE REFERENCE: This line functions, but the timeout doesn't work. The spool simply runs until
    new LowerFeeder(m_intake).withTimeout(5), // the end of autonomous. Unknown if limit switches apply, but they probably do.
    new InstantCommand(() -> m_tower.setTowerSpeedManual(1), m_tower), new ParallelRaceGroup(// keep running the shooter for the whole 15 second teleop
    new RunCommand(() -> m_shooter.shooterVelocity(0.6), m_shooter).withTimeout(14), new SequentialCommandGroup(new ParallelCommandGroup(// [drive to pick up ball (out of initial position)]
    new RunCommand(() -> m_robotDrive.drive(-0.5, 0, 0), m_robotDrive).withTimeout(3), // add a bottom limit argument here [run intake to pick up ball]
    new RunCommand(() -> m_intake.intakeIn(false), m_intake).withTimeout(4), // add a top limit argument here [run tower to keep ball in robot]
    new RunCommand(() -> m_tower.towerMove(true), m_tower).withTimeout(4)), new InstantCommand(() -> m_tower.towerStop(), m_tower), new WaitCommand(1), // [drive to firing position]
    new RunCommand(() -> m_robotDrive.drive(-0.5, 0, 0), m_robotDrive).withTimeout(2), // [tower up (first ball)]
    new RunCommand(() -> m_tower.towerMove(true), m_tower).withTimeout(1), // add bottom limit [tower down (second ball, if there is one)]
    new RunCommand(() -> m_tower.towerMove(false), m_tower).withTimeout(2), // [tower up (second ball, if there is one)]
    new RunCommand(() -> m_tower.towerMove(true), m_tower).withTimeout(2))), new InstantCommand(() -> m_shooter.shooterStop(), m_shooter), new InstantCommand(() -> m_intake.intakeStop(), m_intake));
    CommandScheduler.getInstance().schedule(false, script);
}

Example 5 with WaitCommand

use of edu.wpi.first.wpilibj2.command.WaitCommand in project RapidReact2022 by pittsfordrobotics.

the class RobotContainer method autoConfig.

private void autoConfig() {
    autoChooser.setDefaultOption("No auto", new WaitCommand(0));
    autoChooser.addOption("Run Back", new SequentialCommandGroup(new DrivePathing(Constants.TRAJECTORY_THREE_METER_BACKWARD)));
    autoChooser.addOption("Shoot and Run Back", new AutoShootAndRun());
    autoChooser.addOption("2 Ball Bottom", new AutoFirstBottomLow2());
    autoChooser.addOption("2 Ball Left", new AutoFirstLeftLow2());
    autoChooser.addOption("3 Ball", new AutoSecondLow3());
    ballChooser.setDefaultOption("0", 0);
    ballChooser.addOption("1", 1);
    SmartDashboard.putData("Auto Command", autoChooser);
    SmartDashboard.putData("Starting Balls", ballChooser);
}