Java 类edu.wpi.first.wpilibj.command.PrintCommand 实例源码

public  CG_AutoLowBar() {
         /*Add Commands here:
         e.g. addSequential(new Command1());
              addSequential(new Command2());
         these will run in order.

         To run multiple commands at the same time,
         use addParallel()
         e.g. addParallel(new Command1());
              addSequential(new Command2());
         Command1 and Command2 will run in parallel.

         A command group will require all of the subsystems that each member
         would require.
         e.g. if Command1 requires chassis, and Command2 requires arm,
         a CommandGroup containing them would require both the chassis and the
         arm.*/


        addSequential(new ArmToMax());
        addSequential(new PrintCommand("Autonomous finished"));
//      addSequential(new DriveTime(3, .5));
        addSequential(new Shoot(), 4.0);
        //addSequential(new DriveDistance(10, 500));
    }

public DriveAndShootNoWait(){
    addSequential(new Shift(true));
    addParallel(new PreFire());
    addSequential(new DriveForward(1, 2800));
    addSequential(new SetArmPosition(2));
    addSequential(new PrintCommand("Firing"));
    addSequential(new PrintCommand("Waiting"));
    addSequential(new WaitCommand(1.5));
    addSequential(new PrintCommand("Before Launch"));
    addSequential(new Launch());
    addSequential(new PrintCommand("After Launch"));
    addSequential(new Reset());
}

/** 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"));
    }

/**
 * 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();
}