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

public CenterGearAutonomous() {

        double centerGearAutoSpeed = Preferences.getInstance().getDouble(RobotMap.centerGearAutoSpeed, 0.0);
        double centerGearAutoDistance = Preferences.getInstance().getDouble(RobotMap.centerGearAutoDistance, 0.0);
        double autoWaitTime = Preferences.getInstance().getDouble(RobotMap.autoWaitTime, 0.0);
        double wiggleForward = Preferences.getInstance().getDouble(RobotMap.wiggleForward, 0.0);

        addSequential(new DriveStraightAuto(centerGearAutoDistance, centerGearAutoSpeed));
        addSequential(new TurnAngle(3));
        addSequential(new TurnAngle(-6));
        addSequential(new TurnAngle(3));
        addSequential(new DriveStraightAuto(wiggleForward, centerGearAutoSpeed));
        addSequential(new WaitCommand(autoWaitTime));
        addParallel(new DownManipulator());
        addParallel(new ReverseManipulatorMotors());
        // addSequential(new WaitCommand(autoWaitTime));
        addSequential(new DriveStraightAuto(centerGearAutoDistance / 2, -centerGearAutoSpeed));
        addSequential(new WaitCommand(autoWaitTime / 2));
        addSequential(new DriveStraightAuto(centerGearAutoDistance / 3, centerGearAutoSpeed));
        addSequential(new ManipulatorMotorOff());
        addSequential(new UpManipulator());
    }

public VisionAlignment() {

    table = NetworkTable.getTable("Vision");
    double dist = table.getNumber("est_distance", 0);

    double incr = 0.5;
    int reps = (int) (dist / incr);

    for(int i = 0; i<reps; i++) {
        addSequential(new VisionGyroAlign(), 1.5);
        addSequential(new WaitCommand(0.7));
        addSequential(new DriveForwardDistance(-.2,-.2, -incr/1.5, -incr/1.5, true));
        addSequential(new WaitCommand(0.5));
    }

}

public PaperWeightAuto() {
    // 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 WaitCommand(15));
}

public  FreeFire(boolean menzieShot) {
    //addSequential(new WaitForLock());
    //addSequential(new AutonomousTrack());

    if (menzieShot) {
        addSequential(new MenzieAlign(false));
    } else {
        addSequential(new HorizontalAlign(false));
    }

    addSequential(new MoveShootingWheel(Robot.shootingWheel.CONSTANT_SPEED));
    addSequential(new VerticalAlign(false));
    addSequential(new WaitCommand(0.25));
    addSequential(new FireShooter());
    addSequential(new MoveShootingWheel(0));
}

public  ChevalDeFriseCommand() { //boolean shoot        
//      addParallel(new DriveStraightCommand(60,.5));
//      addSequential(new GetRotation());
//      addSequential(new Pitch(60, .7, 5));
        addSequential(new DriveStraightCommandAndStop(60, .7 , 20));
        addSequential(new WaitCommand(.5));
        addSequential(new DefenseBusterSetpointCommand(Robot.defenseBuster.MAX_VALUE));
        addSequential(new WaitCommand(.45));
        addSequential(new DriveStraightCommand(30,.8));
        addSequential(new DefenseBusterSetpointCommand(Robot.defenseBuster.MIN_VALUE));
        addSequential(new DriveStraightCommand(75,.8));
        // Do vision if shooting.
        //finishAuto(shoot);
        //addSequential(new DriveStraightCommand(30,.9));

        requires(Robot.chassis);
        requires(Robot.defenseBuster);
        //this.shoot = shoot;
    }

public  SingleTote() {

    addSequential(new DoubleAutoCollect());
    addParallel(new DoubleAutoRaiseToteToFirstLevel());
    addSequential(new DoubleAutoTurnToAutoZone());
    addSequential(new WaitCommand(0.5));
    addSequential(new DoubleAutoDriveToAutoZone());

    // 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.
}

public SimpleAutonomous() {
    // 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 WaitCommand(1.0));
    addSequential(new ResetGyroCommand(Math.PI));
    addSequential(new DriveTimeCommand(3.75, 0.167, 0.5, 0.0, -1.0));
    addSequential(new WaitCommand(3.0));
    addSequential(new BunnyLaunch(), 1.0);        
}

public CalibrateWinch() {
    // Calibrates Encoder
    addSequential(new ResetShooterEncoder());
    addSequential(new CalibrateShooterEncoderTop());
    // Shoots the Ball
    addSequential(new ShootBall());
    addSequential(new WaitCommand(.1));
    // Unlatches for when we pull back down
    addSequential(new UnlatchTheLatch());
    // Pulls the winch back up, and calibrates it as the bottom
    addSequential(new PullBackShooter());
    addSequential(new CalibrateShooterEncoderBottom());
    addSequential(new WaitCommand(.1));
    // Latches
    addSequential(new LatchTheLatch());
    addSequential(new WaitCommand(.5));
    // Calibrates then Unwinds
    addSequential(new UnwindWinch());
}

public ShootAndCalibrate() {
    // Shoots the Ball
    addSequential(new ShootBall());
    addSequential(new WaitCommand(.5));
    // Calibrates Encoder
    addSequential(new ResetShooterEncoder());
    addSequential(new CalibrateShooterEncoderTop());
    // Unlatches for when we pull back down
    addSequential(new UnlatchTheLatch());
    // Pulls the winch back up
    addSequential(new PullBackShooter());
    addSequential(new WaitCommand(.1));
    // Latches
    addSequential(new LatchTheLatch());
    addSequential(new WaitCommand(.1));
    // Calibrates then Unwinds
    addSequential(new CalibrateShooterEncoderBottom());
    addSequential(new UnwindWinch());
}

public DriveAndShoot(){
    addParallel(_waitAndDetect());
    addSequential(new Shift(true));
    addSequential(new WaitCommand(0.25));
    addSequential(new DriveForward(1, 2800));
    addSequential(new Conditional(new WaitCommand(.01), new WaitCommand(3)) { //may lower wait time on the waitcommand
        protected boolean condition() {
            return foundHotTarget;
        }
    });
    addSequential(new SetArmPosition(1));
    addParallel(new PreFire());
    addSequential(new SetArmPosition(2));
    addSequential(new WaitCommand(1));
    addSequential(new Launch());
    addSequential(new Reset());
}

public DriveAndShoot2Ball() {
        addSequential(new Shift(true));
        addSequential(new SetLatched(true));
        addSequential(new SetArmPosition(2,false));
        addParallel(new SpinRoller((float) -0.6));
        addSequential(new WaitCommand(0.3));
        addParallel(_waitAndLetRoll());
        addSequential(new DriveForward(1, 4200));
        addSequential(new WaitCommand(1.0));
        addSequential(new WaitForChildren());
//        addSequential(new );
        addSequential(new Launch());
        addParallel(_waitAndIntake());
        addSequential(new Reset());
        addParallel(new PreFire());
        addSequential(new WaitCommand(0.5));
        addSequential(new SpinRoller(0));
        addSequential(new SetArmPosition(0, false));
        addSequential(new WaitCommand(0.5));
        addSequential(new SetArmPosition(2, false));
        addSequential(new WaitCommand(1.0));
        addSequential(new Launch());
        addSequential(new Reset());
    }

protected void initialize() {
    if(condition()) {
        _running = _ifTrue;
    } else {
        _running = _ifFalse;
    }
    if(_running != null) {
        if(_running.getCommand() instanceof WaitCommand) {
            _running.getCommand().start();
        } else {
            _running._initialize();
            _running.initialize();
        }
    }
    _firstRun = true;
}

public TestNets() {
    addSequential(new Output("Starting Net Test"));
    addSequential(new SetState(Subsystems.nets, State.CLOSED, Nets.CLOSE_SPEED));

    addSequential(new SetState(Subsystems.nets.leftNet, State.OPEN, Nets.OPEN_SPEED));

    addSequential(new WaitCommand(3.0d));
    addSequential(new SetState(Subsystems.nets.rightNet, State.OPEN, Nets.OPEN_SPEED));

    addSequential(new WaitCommand(3.0d));
    addSequential(new SetState(Subsystems.nets.rightNet, State.CLOSED, Nets.CLOSE_SPEED));

    addSequential(new WaitCommand(3.0d));
    addSequential(new SetState(Subsystems.nets.leftNet, State.CLOSED, Nets.CLOSE_SPEED));

    addSequential(new WaitCommand(3.0d));
    addSequential(new SetState(Subsystems.nets, State.OPEN, Nets.OPEN_SPEED));

    addSequential(new WaitCommand(3.0d));
    addSequential(new SetState(Subsystems.nets, State.CLOSED, Nets.CLOSE_SPEED));

    addSequential(new Output("Net Test Complete"));
}

public DriveBox() {
    requires(Subsystems.driveTrain);
    addSequential(new DriveAtSpeed(0.3d),1.0d);
    addSequential(new WaitCommand(0.2));
    addSequential(new TurnRelativeAngle(90));
    addSequential(new WaitCommand(0.2));
    addSequential(new DriveAtSpeed(0.3d),1.0d);
    addSequential(new WaitCommand(0.2));
    addSequential(new TurnRelativeAngle(90));
    addSequential(new WaitCommand(0.2));
    addSequential(new DriveAtSpeed(0.3d),1.0d);
    addSequential(new WaitCommand(0.2));
    addSequential(new TurnRelativeAngle(90));
    addSequential(new WaitCommand(0.2));
    addSequential(new DriveAtSpeed(0.3d),1.0d);
    addSequential(new WaitCommand(0.2));
    addSequential(new TurnRelativeAngle(90));
    addSequential(new WaitCommand(0.2));
}

public AutonomousCommand() {
    setTimeout(15);

    // Calibrates the shooter by moving it all the way down
    addSequential(new AutonomousCalibrate());

    // Sets the angle to Autonomous, so it can shoot
    addSequential(new AutonomousSetAngle());

    // Speeds up the shooter motors
    addParallel(new AutonomousSpeedUp()); // Initially spins up shooter motors
    // Waits for the motors to spin up or it to "timeout", and
    // waits for the angle to get to the setpoint
    addSequential(new WaitOrShoot(5));
    // Waits an extra second for good measure
    addSequential(new WaitCommand(1));

    // Shoots the frisbees
    addSequential(new AutonomousShootFrisbees(0.2, 1));

    // Spins down the motors
    addSequential(new AutonomousSpeedDown());

    // Do a dance
    addSequential(new AutonomousDance());
}

public SimpleAutonomous() {
    // 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 WaitCommand(1.0));
    addSequential(new ResetGyroCommand(Math.PI));
    addSequential(new DriveTimeCommand(3.75, 0.167, 0.5, 0.0, -1.0));
    addSequential(new WaitCommand(3.0));
    addSequential(new BunnyLaunch(), 1.0);        
}

public BoilerSideBlueAuto() {
    addSequential(new DriveStraightAuto(LeftGearAutoDist, centerGearAutoSpeed));
    addSequential(new TurnAngle(45));
    addSequential(new WaitCommand(autoWaitTime));
    addSequential(new DriveStraightAuto(driveToAngledPegDistance, centerGearAutoSpeed));
    addParallel(new DownManipulator());
    addParallel(new ReverseManipulatorMotors());
    addSequential(new DriveStraightAuto(driveToAngledPegDistance - 10, -centerGearAutoSpeed));
    addSequential(new ManipulatorMotorOff());
    addSequential(new UpManipulator());
}

public BoilerSideRedAuto() {
    addSequential(new DriveStraightAuto(LeftGearAutoDist, centerGearAutoSpeed));
    addSequential(new TurnAngle(-45));
    addSequential(new WaitCommand(autoWaitTime));
    addSequential(new DriveStraightAuto(driveToAngledPegDistance, centerGearAutoSpeed));
    addParallel(new DownManipulator());
    addParallel(new ReverseManipulatorMotors());
    addSequential(new DriveStraightAuto(driveToAngledPegDistance - 10, -centerGearAutoSpeed));
    addSequential(new ManipulatorMotorOff());
    addSequential(new UpManipulator());
}

public EmergencyTimeAutonomous() {
    addSequential(new DriveTime(.386, 1.0));
    addSequential(new WaitCommand(1));
    addSequential(new DownManipulator());
    addSequential(new WaitCommand(1));
    addSequential(new DriveTime(.294, -1.0));
    addSequential(new UpManipulator());
    addSequential(new WaitCommand(1));
    addSequential(new TurnAngle(90));
    addSequential(new DriveTime(1, 1.0));
    addSequential(new TurnAngle(-90));
    addSequential(new DriveTime(.3, 1.0));
}

public CenterNoVision() {
//          addSequential(new DFDSpeed(-200, -200, 2, 2));
//          addSequential(new WaitCommand(0.5));
//          addSequential(new GearOn(false, true), 1);
//          addSequential(new WaitCommand(0.5));
//          addSequential(new DFDSpeed(200, 200, 1.4, 1.4));
            addSequential(new GyroTurn(-150, 50));
            addSequential(new WaitCommand(0.5));
            addSequential(new GyroTurn(-150, -50));
    }

public LeftPeg() {

        Robot.driveTrain.resetEnc();
        table = NetworkTable.getTable("Vision");

        addSequential(new DFDSpeed(-200, -200, 1.55, 1.55));
        addSequential(new WaitCommand(0.2));
        addSequential(new GyroTurn(-150, 50), 2);
        addSequential(new WaitCommand(0.2));
        addSequential(new VisionGyroAlign(), 1);
        addSequential(new WaitCommand(0.2));
        addSequential(new MovingVisionAlignment(), 5); //removed timeout
        addSequential(new WaitCommand(0.5));

        addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment(), 5); //removed timeout
        addSequential(new WaitCommand(0.1));
        addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment(), 5); //removed timeout
        addSequential(new WaitCommand(0.1));
        addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment()); //removed timeout
        addSequential(new WaitCommand(0.1));
        addSequential(new GearOn(false, false), 1);
        addSequential(new WaitCommand(0.5));
        addSequential(new DFDSpeed(200, 200, 1.4, 1.4));

    }

public RightPeg() {
    Robot.driveTrain.resetEnc();
    table = NetworkTable.getTable("Vision");

    addSequential(new DFDSpeed(-200, -200, 1.60, 1.60));
    addSequential(new WaitCommand(0.2));
    addSequential(new GyroTurn(-150, -50), 2); //CHECK + AND -
    addSequential(new WaitCommand(0.2));
    addSequential(new VisionGyroAlign(), 1);
    addSequential(new WaitCommand(0.2));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.2));

    //Move back retry
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment()); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new GearOn(false, false), 1);
    addSequential(new WaitCommand(0.5));
    addSequential(new DFDSpeed(200, 200, 1.4, 1.4));

}

public CenterVision() {
        addSequential(new DFDSpeed(-200, -200, .75, .75));
        addSequential(new WaitCommand(0.1));
        addSequential(new MovingVisionAlignment(), 6);
        addSequential(new WaitCommand(0.5));
        addSequential(new GearOn(false, false), 1);
        addSequential(new WaitCommand(0.5));
        addSequential(new DFDSpeed(200, 200, 1.4, 1.4));
}

public MidGearAuto() {
    System.out.println("Placing mid gear");
    addSequential(new CalibrateGyro());
    addSequential(new CloseGearManipulator());
    addSequential(new ConstantDrive(.5, 2),2);
    addSequential(new WaitCommand(0.5));
    addSequential(new Rotate(0));
    /*addSequential(new GyroDistanceDrive(1*12),1);
    addSequential(new WaitCommand(.5));
    addSequential(new OpenGearManipulator());
    addSequential(new WaitCommand(.5));
    addSequential(new ConstantDrive(-.5,1));*/
    //(This stays commented out) addSequential(new SoftwareDistanceDrive(-3 * 12));
    addSequential(new CloseGearManipulator());
}

public RightGearAuto() {
    System.out.println("Placing right gear");
    addSequential(new CloseGearManipulator());
    addSequential(new GyroDistanceDrive(7 * 12),4);
    addSequential(new WaitCommand(1));
    addSequential(new RotateRelative(-60), 4); // Turn left to face peg
    addSequential(new WaitCommand(0.4)); // Small delay after turning
    addSequential(new GyroDistanceDrive(2.6 * 12 ), 4); // Drive up to peg
    addSequential(new OpenGearManipulator()); // Drop gear
    addSequential(new WaitCommand(1)); // Wait for gear to settle
    addSequential(new ConstantDrive(-0.5, 0.5)); // Move backwards
    addSequential(new CloseGearManipulator()); // Get the robot ready for teleop
}

public RedShootAuto()
{
    addSequential(new Shoot(), 4);
    addSequential(new BackwardsAgitatorCommand(), 0.5);
    addSequential(new Shoot(), 4.5);
    addSequential(new ConstantDrive(-0.4, 1));
    addSequential(new RotateRelative(80), 1.5);
    addSequential(new WaitCommand(0.3));
    addSequential(new DistanceDrive(9 * 12));
}

public BlueShootAuto()
{
    addSequential(new Shoot(), 4);
    addSequential(new BackwardsAgitatorCommand(), 0.5);
    addSequential(new Shoot(), 4);
    addSequential(new BackwardsAgitatorCommand(), 0.5);
    addSequential(new ConstantDrive(-0.4, 1));
    addSequential(new RotateRelative(-60), 1.5);
    addSequential(new WaitCommand(0.3));
    addSequential(new DistanceDrive(9 * 12));
}

@Override
public void autonomousInit() {
    Scheduler.getInstance().removeAll();
    new ShiftDown().start();
    // Attempt to prevent half the talons from cutting out
    new WaitCommand(0.1).start();
    new ConstantDrive(0, 0.1);

    autoSelector.getSelected().start();
}

public SlowMediumRangeShot() {
    addSequential(new UnlockShooter()); // Release shooter piston
    // addSequential(new SetShooterSpeed(.9)); // Set shooter wheel PIDs
    addSequential(new SetShooterSpeed(.9));
    addParallel(new AimParallel());
    addSequential(new TurnToGoalWithGyroSlow()); // see TurnToGoalWithGyro()
    // addSequential(new WaitCommand(.5));
    addSequential(new Aim()); // Ajusts shooter angle based on distance algorithm
    addSequential(new MoveBallIntoStorage());
    addSequential(new MoveBallIntoShooter()); // Drive feeder roller untill ball leaves
    addSequential(new WaitCommand(.25)); // Guarantee ball has left
    addSequential(new SetShooterSpeed(0.0)); // Spin down shooter
    addSequential(new CancelShot()); // Free up shooting subsystems
    addSequential(new DriveWithJoysticks()); // Return control to the driver
}

public MidCDFAuto() {

        addSequential(new SetShooterSpeed(.9));
        addSequential(new MoveActuatorsUp());
        addSequential(new DriveDistance(.5, 42));
        addSequential(new LowerAManipulators(), 2);
        addSequential(new WaitCommand(.5));
        addSequential(new DriveDistance(-.4, -6));
        addSequential(new DriveDistance(.7, 102));
        addSequential(new UnlockShooter());
        addSequential(new MediumRangeShot());
        // addSequential(new ToggleDriveDirection());
    }

public MediumRangeShot() {
    addSequential(new UnlockShooter()); // Release shooter piston
    // addSequential(new SetShooterSpeed(.9)); // Set shooter wheel PIDs
    addSequential(new SetShooterSpeed(.9));
    addParallel(new AimParallel());
    addSequential(new TurnToGoalWithGyro()); // see TurnToGoalWithGyro()
    // addSequential(new WaitCommand(.5));
    addSequential(new Aim()); // Ajusts shooter angle based on distance algorithm
    addSequential(new MoveBallIntoStorage());
    addSequential(new MoveBallIntoShooter()); // Drive feeder roller untill ball leaves
    addSequential(new WaitCommand(.25)); // Guarantee ball has left
    addSequential(new SetShooterSpeed(0.0)); // Spin down shooter
    addSequential(new CancelShot()); // Free up shooting subsystems
    addSequential(new DriveWithJoysticks()); // Return control to the driver
}

public EjectBall() {
    addSequential(new RaiseRake());
    addParallel(new ReverseRoller());
    addSequential(new IntakeMotorReverse());
    addSequential(new WaitCommand(1.0)); // maybe change this
    addSequential(new StopRoller());
    addSequential(new IntakeMotorStop());
}

public BackwardMovingShot() {
    addSequential(new UnlockShooter()); // Release shooter piston
    addSequential(new SetShooterSpeed(.9)); // Set shooter wheel PIDs
    addParallel(new AimParallel());
    addSequential(new TurnToGoalWhileDrivingBackward()); // see TurnToGoalWhileDrivingBackward()
    addSequential(new WaitCommand(.5));
    addSequential(new Aim()); // Ajusts shooter angle based on distance algorithm
    // addSequential(new WaitCommand(.5)); // Waits for shooter to settle
    addSequential(new MoveBallIntoShooter()); // Drive feeder roller untill ball leaves
    addSequential(new WaitCommand(.25)); // Guarantee ball has left
    addSequential(new SetShooterSpeed(0.0)); // Spin down shooter
    addSequential(new CancelShot()); // Free up shooting subsystems
    addSequential(new DriveWithJoysticks()); // Return control to the driver
}

public ForwardMovingShot() {
    addSequential(new UnlockShooter()); // Release shooter piston
    addSequential(new SetShooterSpeed(.9)); // Set shooter wheel PIDs
    addParallel(new AimParallel());
    addSequential(new TurnToGoalWhileDrivingForward()); // see TurnToGoalWhileDrivingForward()
    addSequential(new WaitCommand(.5));
    addSequential(new Aim()); // Ajusts shooter angle based on distance algorithm
    // addSequential(new WaitCommand(.5)); // Waits for shooter to settle
    addSequential(new MoveBallIntoShooter()); // Drive feeder roller untill ball leaves
    addSequential(new WaitCommand(.25)); // Guarantee ball has left
    addSequential(new SetShooterSpeed(0.0)); // Spin down shooter
    addSequential(new CancelShot()); // Free up shooting subsystems
    addSequential(new DriveWithJoysticks()); // Return control to the driver
}

public BatterShot() {
    addSequential(new DriveDistance(-.4, 26));
    addSequential(new UnlockShooter());
    addSequential(new LowerRake());
    addSequential(new SetShooterSpeed(Preferences.getInstance().getDouble("ShooterSpeed", 0.0)));
    addSequential(new AimToAngle(61));
    addSequential(new WaitCommand(1.0));
    addSequential(new MoveBallIntoShooter());
    addSequential(new WaitCommand(1.0));
    addSequential(new SetShooterSpeed(0.0));
    addSequential(new RaiseRake());

}

public ShootIntoGoal() {

        // Arms down
        addSequential(new ArmsDown());

        // Aim at goal
        addSequential(new ManualShooterAngle(), 0.7);

        // Turn to goal
        addSequential(new RotateRobotToGoal(), 0.8);
        addSequential(new WaitCommand(0.5));
        addSequential(new RotateRobotToGoal(), 0.5);
        addSequential(new WaitCommand(0.5));
        addSequential(new RotateRobotToGoal(), 0.3);
        addSequential(new WaitCommand(0.5));
        addSequential(new RotateRobotToGoal(), 0.3);

        // Aim at goal
        addSequential(new SetShooterAngleToGoal(), 0.7);

//      // Rev up shooter
//      addSequential(new SetShooterToCalculatedSpeed(), 1.5);
//      
//      // Fire at goal
//      addSequential(new ToggleShooterPiston());
//      addSequential(new WaitCommand(0.25));
//      addSequential(new ToggleShooterPiston());

        addSequential(new ShootFullSpeed());

    }

public LaunchBallCommandGroup() {
    System.out.println("Launch Ball Command Group");
    addSequential(new ActivateLauncherServosCommand());
    addSequential(new WaitCommand(1));
    addSequential(new RetractLauncherServosCommand());
    addSequential(new StopWheelsCommand());
}

public ChevalDeFrise(IntakeSide intakeSide) {
    addSequential(new SetVerticalIntake(80, intakeSide));
    addParallel(new AssistedDrive(AssistedTranslateType.ENCODER, AssistedRotateType.ENCODER, 24, 0, 12));
    addSequential(new WaitCommand(1));
    addParallel(new SetVerticalIntake(20, intakeSide)); // Slowly lift arm as robot moves across
    addParallel(new AssistedDrive(AssistedTranslateType.ENCODER, AssistedRotateType.ENCODER, 12, 0, 12)); 
}

public  GiveBallToShooter(IntakeSide intakeSide) {
    if (intakeSide == IntakeSide.FRONT) {
        addParallel(new MoveTurnTable(0));
    } else {
        addParallel(new MoveTurnTable(180));
    }
    //addParallel(new MoveTurnTable((intakeSide == IntakeSide.FRONT) ? 180 : 0));
    addSequential(new MoveHood(25));
    addSequential(new SetVerticalIntake(20, intakeSide));
    addSequential(new SpinIntake(Direction.FORWARD, 1, IntakeSide.FRONT));
    //addSequential(new CheckIntakeBreakBeam(intakeSide, true, true, 0));
    addSequential(new WaitCommand(1));
    addSequential(new MoveHood(Robot.hood.HOOD_MIN)); // Forward would be positive degrees. This command traps the ball
}

public  CalibrateVisionAngle() {
    addSequential(new MoveTurnTable(Robot.turntable.CALIBRATION_START));

    for (double currentAngle = Robot.turntable.CALIBRATION_START; currentAngle < -Robot.turntable.CALIBRATION_START; currentAngle += Robot.turntable.CALIBRATION_INCREMENT) {
        addSequential(new MoveTurnTable(currentAngle));
        addSequential(new WaitCommand(0.5));
        addSequential(new CompareVisionAngle());
    }
}