Zed.java 文件源码

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