java类edu.wpi.first.wpilibj.command.CommandGroup的实例源码

public void run() {
    TestCommand command1 = new TestCommand();
    TestCommand command2 = new TestCommand();

    CommandGroup commandGroup = new CommandGroup();
    commandGroup.addParallel(command1);
    commandGroup.addParallel(command2);


    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    commandGroup.start();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 0, 0);
    assertCommandState(command2, 1, 2, 2, 0, 0);
    commandGroup.cancel();
    assertCommandState(command1, 1, 2, 2, 0, 0);
    assertCommandState(command2, 1, 2, 2, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 0, 1);
    assertCommandState(command2, 1, 2, 2, 0, 1);

}

@JsonIgnore
abstract void setParent(CommandGroup parent);

@Override
protected CommandGroup createAutonomousCommand()
{
    return null;
}

@Override
protected CommandGroup createAutonomousCommand()
{
    return mAutonFactory.createAutonMode();
}

public Command assembleCommand() {
    CommandGroup cmd = new CommandGroup();
    cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.5));// Move to the start defense
    switch (startDefense()) {// Traverse the start defense
    case LOW_BAR:
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.5));
        break;
    case PORTCULLIS:
        cmd.addSequential(new TraversePortcullis(true));//Travels 50
        cmd.addSequential(new DriveStraight(10 * (2.0 + 4.0 / 9.0), 0, 0.5));
        break;
    case CHEVAL_DE_FRESE:
        return null;
    case SALLY_PORT:
        return null;
    case DRAWBRIDGE:
        return null;
    case ROUGH_TERRAIN:
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.3));
        break;
    case RAMPARTS:
        return null;
    case ROCK_WALL:
        cmd.addSequential(new DriveStraight(70 * (2.0 + 4.0 / 9.0), 0, 0.8));
        break;
    case MOAT:
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.7));
        break;
    }
    //Move to firing position
    switch (startPosition()) {
    case(1):
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0, 0.5));
        cmd.addSequential(new TurnToAtRate(46, 0.5));
        break;
    case(2):
        cmd.addSequential(new DriveStraight(130 * (2.0 + 4.0 / 9.0), 0, 0.5));
        cmd.addSequential(new TurnToAtRate(46, 0.5));
        break;
    case(3):
        cmd.addSequential(new TurnToAtRate(45, 0.5));
        cmd.addSequential(new DriveStraight(45 * (2.0 + 4.0 / 9.0), 0.5));
        cmd.addSequential(new TurnToAtRate(0, 0.5));
        break;
    case(4):
        cmd.addSequential(new DriveStraight(20 * (2.0 + 4.0 / 9.0), 0, 0.5));
        break;
    case(5):
        cmd.addSequential(new TurnToAtRate(335, 0.5));
        cmd.addSequential(new DriveStraight(60 * (2.0 + 4.0 / 9.0), 0.5));
        cmd.addSequential(new TurnToAtRate(0, 0.5));
        break;
    }
    cmd.addSequential(new VisionSeek());
    cmd.addSequential(new LoadBoulder());
    return cmd;
}

public CommandGroup getAutonomousCommand() {
    return currentAutonomousCommand;
}

/** Called on robot boot. */
    public void robotInit() {
        catapult   = new Catapult();
        driveTrain = new DriveTrain();
        leds       = new LEDStrip();
        intake     = new Intake();
        ledring    = new LEDRing();
        staticleds = new StaticLEDStrip();
        compressor = new Compressor(RobotMap.PORT_SWITCH_COMPRESSO, RobotMap.PORT_RELAY_COMPRESSOR);
        compressor.start();

        // Initialize OI last so it doesn't try to access null subsystems
        oi         = new OI();

        //SmartDashboard.putBoolean("Wait longer", true);
        SmartDashboard.putData("Arms out", new SetArmPosition(2));
        SmartDashboard.putData("Arms in", new SetArmPosition(0));
//        SmartDashboard.putData("Prefire", new PreFire());
        CommandGroup armsMoveWait = new CommandGroup();
        armsMoveWait.addSequential(new PrintCommand("Arms up"));
        armsMoveWait.addSequential(new SetArmPosition(0, false));
        armsMoveWait.addSequential(new PrintCommand("Arms are up"));
        armsMoveWait.addSequential(new WaitCommand(0.5));
        armsMoveWait.addSequential(new PrintCommand("Arms down"));
        armsMoveWait.addSequential(new SetArmPosition(2, false));
        armsMoveWait.addSequential(new PrintCommand("Arms are down"));
        SmartDashboard.putData("Arms move wait", armsMoveWait);
        CommandGroup armsMoveNoWait = new CommandGroup();
        armsMoveNoWait.addSequential(new SetArmPosition(0, false));
        armsMoveNoWait.addSequential(new SetArmPosition(2, false));
        SmartDashboard.putData("Arms move no wait", armsMoveNoWait);
        SmartDashboard.putData("Arms in quick", new SetArmPosition(0,false));

        // The names, and corresponding Commands of our autonomous modes
        autonomiceNames = new String[]{"Drive Forward","1 Ball Hot","1 Ball Blind","2 Ball"};
        autonomice = new Command[]{new DriveForward(0.8, 5250),new DriveAndShoot(),new DriveAndShootNoWait(),new DriveAndShoot2Ball()};

        // Configure and send the SendableChooser, which allows autonomous modes
        // to be chosen via radio button on the SmartDashboard
        System.out.println(autonomice.length + " autonomice");
        for (int i = 0; i < autonomice.length; ++i) {
            chooser.addObject(autonomiceNames[i], autonomice[i]);
        }
        SmartDashboard.putData("Which Autonomouse?", chooser);
        SmartDashboard.putData(Scheduler.getInstance());

        /*SmartDashboard.putData("Test conditional", new Conditional(new WaitCommand(0.5), new WaitCommand(3.0)) {
            protected boolean condition() {
               return SmartDashboard.getBoolean("Wait longer", false);
            }
        });*/
        // Send sensor info to the SmartDashboard periodically
        new Command("Sensor feedback") {
            protected void initialize() {}
            protected void execute() {
                sendSensorData();
            }
            protected boolean isFinished() {
                return false;
            }
            protected void end() {}
            protected void interrupted() {
                end();
            }
        }.start();
        leds.initTable(NetworkTable.getTable("SmartDashboard"));
        ledring.initTable(NetworkTable.getTable("SmartDashboard"));
        staticleds.initTable(NetworkTable.getTable("SmartDashboard"));
    }

public static CommandGroup doNothingAutonomous() {
    return new CommandGroup();
}

/**
 * This function is called once at the start of autonomous mode.
 */
public void autonomousInit(){
    DriverStation driverStation = DriverStation.getInstance();
    double delayTime = driverStation.getAnalogIn(1);

    boolean trackHighGoal = driverStation.getDigitalIn(1);
    boolean trackMiddleGoal = driverStation.getDigitalIn(2);
    boolean shootInAuto = true;

    betweenModes();
    DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;
    drivetrainStrafe.setDefaultCommand(new MaintainStateCommand(drivetrainStrafe));
    DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;
    drivetrainRotation.setDefaultCommand(new MaintainStateCommand(drivetrainRotation));

    CommandGroup fastAugerSequence = new CommandGroup();
    fastAugerSequence.addSequential(new PrintCommand("Dispensing auger"));
    fastAugerSequence.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
    fastAugerSequence.addSequential(new WaitCommand(0.8));

    CommandGroup augerSequence = new CommandGroup();
    augerSequence.addSequential(new PrintCommand("Dispensing auger"));
    augerSequence.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
    augerSequence.addSequential(new WaitCommand(2));

    CommandGroup firstAugerDrop = new CommandGroup();
    firstAugerDrop.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
    firstAugerDrop.addSequential(new WaitCommand(0.5));
    firstAugerDrop.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));

    CommandGroup autoCommand = new CommandGroup();
    autoCommand.addSequential(new PrintCommand("Starting autonomous"));
    autoCommand.addSequential(new WaitCommand(delayTime));
    if(shootInAuto){
        autoCommand.addSequential(new FixedPointVisionTrackingCommand(FixedPointVisionTrackingCommand.PYRAMID_BACK_MIDDLE, TargetType.HIGH_GOAL), 4);
        autoCommand.addParallel(firstAugerDrop);
        autoCommand.addSequential(new SetShooterCommand(Shooter.SHOOTER_ON));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new SetConveyorCommand(Conveyor.CONVEYOR_SHOOT_IN));
        autoCommand.addSequential(new WaitCommand(1));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(1.75));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(1.75));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(1.75));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new PrintCommand("Dispensing auger"));
        autoCommand.addSequential(new AugerRotateCommand(Auger.AugerDirection.AUGER_DOWN));
        autoCommand.addSequential(new WaitCommand(2));
        autoCommand.addSequential(new WaitForChildren());
        autoCommand.addSequential(new WaitCommand(2));
    }
    //autoCommand.addSequential(new RepeatCommand(new PrintCommand("Testing print"), 5));
    //autoCommand.addSequential(augerSequence, 5);

    autonomousCommand = autoCommand;
    autonomousCommand.start();
}

public void run() {
    final ASubsystem subsystem = new ASubsystem();


    TestCommand command1 = new TestCommand() {
        {
            requires(subsystem);
        }
    };

    TestCommand command2 = new TestCommand() {
        {
            requires(subsystem);
        }
    };

    TestCommand command3 = new TestCommand() {
        {
            requires(subsystem);
        }
    };

    CommandGroup commandGroup = new CommandGroup();
    commandGroup.addSequential(command1);
    commandGroup.addSequential(command2);
    commandGroup.addSequential(command3);


    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    commandGroup.start();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 0, 0, 0, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    command1.setHasFinished(true);
    assertCommandState(command1, 1, 1, 1, 0, 0);
    assertCommandState(command2, 0, 0, 0, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);

    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);
    command2.setHasFinished(true);
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 1, 1, 0, 0);
    assertCommandState(command3, 0, 0, 0, 0, 0);

    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 1, 1, 0, 0);
    command3.setHasFinished(true);
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 1, 1, 0, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 2, 2, 1, 0);
    Scheduler.getInstance().run();
    assertCommandState(command1, 1, 2, 2, 1, 0);
    assertCommandState(command2, 1, 2, 2, 1, 0);
    assertCommandState(command3, 1, 2, 2, 1, 0);

}