/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

package storm2014.commands.autonomous;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.PrintCommand;
import edu.wpi.first.wpilibj.command.WaitCommand;
import edu.wpi.first.wpilibj.command.WaitForChildren;
import storm2014.Robot;
import storm2014.commands.DriveForward;
import storm2014.commands.Launch;
import storm2014.commands.PreFire;
import storm2014.commands.Reset;
import storm2014.commands.SetArmPosition;
import storm2014.commands.Shift;
import storm2014.commands.SpinRoller;
import storm2014.commands.WaitForEncoder;
import storm2014.subsystems.Catapult;

/**
 *
 * @author Tim
 */
public class DriveAndShoot2Ball extends CommandGroup{
    
    private Command _waitAndRoll() {
        CommandGroup _waitAndRoll = new CommandGroup("Waits then rolls in the spinner");
        _waitAndRoll.addSequential(new WaitCommand(1.25));
        _waitAndRoll.addSequential(new SpinRoller(-1));
        return _waitAndRoll;
    }
    
    private Command _waitAndLetRoll(){
        CommandGroup _waitAndPrefire = new CommandGroup();
        _waitAndPrefire.addSequential(new WaitCommand(1.75));
        _waitAndPrefire.addParallel(new PreFire());
        _waitAndPrefire.addSequential(new WaitCommand(1.25));
        CommandGroup _waitAndLetRoll = new CommandGroup("rolls ball out of way");
        _waitAndLetRoll.addParallel(_waitAndPrefire);
        _waitAndLetRoll.addSequential(new WaitForEncoder(4200 - 300));
        _waitAndLetRoll.addSequential(new SpinRoller(0));
        return _waitAndLetRoll;
    }
    
    public DriveAndShoot2Ball() {
        addSequential(new Shift(true));
        addSequential(new SetArmPosition(2));
        addParallel(new SpinRoller((float) -0.6));
        addSequential(new WaitCommand(0.3));
        addParallel(_waitAndLetRoll());
        addSequential(new DriveForward(1, 4200));
        addSequential(new WaitForChildren());
        addSequential(new WaitCommand(1.0));
        addSequential(new Launch());
        addParallel(_waitAndRoll());
        addSequential(new Reset());
        addParallel(new PreFire());
        addSequential(new WaitCommand(0.5));
        addSequential(new SpinRoller(0));
        addSequential(new SetArmPosition(0));
        addSequential(new WaitCommand(0.75));
        addSequential(new SetArmPosition(2));
        addSequential(new WaitCommand(1.0));
        addSequential(new Launch());
        addSequential(new Reset());
    }
    
}

@@ -1351,17 +1351,16 @@
 mmand(1.
-7
 5));%0A   
@@ -1412,70 +1412,8 @@
 ));%0A
-        _waitAndPrefire.addSequential(new WaitCommand(1.25));%0A
     
@@ -2026,54 +2026,8 @@
 ));%0A
-        addSequential(new WaitForChildren());%0A
     

Fix WaitForChildren bug